From 7f51d63380965e56ede39018ef6720242e1aee73 Mon Sep 17 00:00:00 2001 From: Philipp Kramer Date: Wed, 11 Sep 2024 11:40:46 +0200 Subject: [PATCH] copy over behaviour template --- prosthesis_controller/SimplexNoise.cpp | 114 +++++++++++ prosthesis_controller/SimplexNoise.h | 54 +++++ prosthesis_controller/behaviour.cpp | 186 ++++++++++++++++++ prosthesis_controller/behaviour.h | 43 ++++ prosthesis_controller/bodytemp.cpp | 57 ++++++ prosthesis_controller/bodytemp.h | 13 ++ prosthesis_controller/definitions.h | 7 +- .../prosthesis_controller.ino | 39 +++- prosthesis_controller/servo.cpp | 14 +- prosthesis_controller/servo.h | 7 + 10 files changed, 517 insertions(+), 17 deletions(-) create mode 100644 prosthesis_controller/SimplexNoise.cpp create mode 100644 prosthesis_controller/SimplexNoise.h create mode 100644 prosthesis_controller/behaviour.cpp create mode 100644 prosthesis_controller/behaviour.h create mode 100644 prosthesis_controller/bodytemp.cpp create mode 100644 prosthesis_controller/bodytemp.h diff --git a/prosthesis_controller/SimplexNoise.cpp b/prosthesis_controller/SimplexNoise.cpp new file mode 100644 index 0000000..63c4110 --- /dev/null +++ b/prosthesis_controller/SimplexNoise.cpp @@ -0,0 +1,114 @@ +/* +=============================================================================== +Ported the SimplexNoise algorithm from the C++ versions mentioned below +to a reuseable Arduino Library. + +By Jordan Shaw / http://jordanshaw.com / 2017-02 + +A C++ port of a speed-improved simplex noise algorithm for 2D in Java. +Based on example code by Stefan Gustavson (stegu@itn.liu.se). +Optimisations by Peter Eastman (peastman@drizzle.stanford.edu). +Better rank ordering method by Stefan Gustavson in 2012. +C++ port and minor type and algorithm changes by Josh Koch (jdk1337@gmail.com). +This could be speeded up even further, but it's useful as it is. +Version 2012-04-12 +The original Java code was placed in the public domain by its original author, +Stefan Gustavson. You may use it as you see fit, +but attribution is appreciated. + +Original gist url: https://gist.github.com/Slipyx/2372043 + +=============================================================================== +*/ + +#include +#include "Arduino.h" +#include "SimplexNoise.h" + +// Private static member definitions +const double SimplexNoise::F2 = 0.5 * (sqrt( 3.0 ) - 1.0); +const double SimplexNoise::G2 = (3.0 - sqrt( 3.0 )) / 6.0; +const uint8_t SimplexNoise::p[256] = { + 151,160,137,91,90,15,131,13,201,95,96,53,194,233,7,225,140,36,103,30,69, + 142,8,99,37,240,21,10,23,190,6,148,247,120,234,75,0,26,197,62,94,252,219, + 203,117,35,11,32,57,177,33,88,237,149,56,87,174,20,125,136,171,168,68,175, + 74,165,71,134,139,48,27,166,77,146,158,231,83,111,229,122,60,211,133,230, + 220,105,92,41,55,46,245,40,244,102,143,54,65,25,63,161,1,216,80,73,209,76, + 132,187,208,89,18,169,200,196,135,130,116,188,159,86,164,100,109,198,173, + 186,3,64,52,217,226,250,124,123,5,202,38,147,118,126,255,82,85,212,207,206, + 59,227,47,16,58,17,182,189,28,42,223,183,170,213,119,248,152,2,44,154,163, + 70,221,153,101,155,167,43,172,9,129,22,39,253,19,98,108,110,79,113,224,232, + 178,185,112,104,218,246,97,228,251,34,242,193,238,210,144,12,191,179,162, + 241,81,51,145,235,249,14,239,107,49,192,214,31,181,199,106,157,184,84,204, + 176,115,121,50,45,127,4,150,254,138,236,205,93,222,114,67,29,24,72,243,141, + 128,195,78,66,215,61,156,180 +}; +const Grad SimplexNoise::grad3[12] = { + Grad(1,1,0),Grad(-1,1,0),Grad(1,-1,0),Grad(-1,-1,0),Grad(1,0,1), + Grad(-1,0,1),Grad(1,0,-1),Grad(-1,0,-1),Grad(0,1,1),Grad(0,-1,1), + Grad(0,1,-1),Grad(0,-1,-1) +}; +uint8_t SimplexNoise::perm[512] = {0}; +uint8_t SimplexNoise::permMod12[512] = {0}; + +// Initialize permutaion arrays +void SimplexNoise::init() { + for ( uint16_t i = 0; i < 512; ++i ) { + perm[i] = p[i & 255]; + permMod12[i] = static_cast(perm[i] % 12); + } + delete[] &p; // lol what +} + +// Fast floor +int32_t SimplexNoise::fastFloor( double x ) { + int32_t xi = static_cast(x); + return x < xi ? xi - 1 : xi; +} + +double SimplexNoise::dot( const Grad& g, double x, double y ) { + return g.x * x + g.y * y; +} + +// 2D SimplexNoise noise +double SimplexNoise::noise( double xin, double yin ) { + double s = (xin + yin) * F2; + int32_t i = fastFloor( xin + s ); + int32_t j = fastFloor( yin + s ); + double t = (i + j) * G2; + double x0 = xin - (i - t); + double y0 = yin - (j - t); + uint8_t i1 = 0, j1 = 1; + if ( x0 > y0 ) { + i1 = 1; + j1 = 0; + } + double x1 = x0 - i1 + G2; + double y1 = y0 - j1 + G2; + double x2 = x0 - 1.0 + 2.0 * G2; + double y2 = y0 - 1.0 + 2.0 * G2; + uint8_t ii = i & 255; + uint8_t jj = j & 255; + uint8_t gi0 = permMod12[ii + perm[jj]]; + uint8_t gi1 = permMod12[ii + i1 + perm[jj + j1]]; + uint8_t gi2 = permMod12[ii + 1 + perm[jj + 1]]; + double n0 = 0.0; + double t0 = 0.5 - x0 * x0 - y0 * y0; + if ( t0 >= 0.0 ) { + t0 *= t0; + n0 = t0 * t0 * dot( grad3[gi0], x0, y0 ); + } + double n1 = 0.0; + double t1 = 0.5 - x1 * x1 - y1 * y1; + if ( t1 >= 0.0 ) { + t1 *= t1; + n1 = t1 * t1 * dot( grad3[gi1], x1, y1 ); + } + double n2 = 0.0; + double t2 = 0.5 - x2 * x2 - y2 * y2; + if ( t2 >= 0.0 ) { + t2 *= t2; + n2 = t2 * t2 * dot( grad3[gi2], x2, y2 ); + } + return 70.0 * (n0 + n1 + n2); +} \ No newline at end of file diff --git a/prosthesis_controller/SimplexNoise.h b/prosthesis_controller/SimplexNoise.h new file mode 100644 index 0000000..4ed97af --- /dev/null +++ b/prosthesis_controller/SimplexNoise.h @@ -0,0 +1,54 @@ +/* +=============================================================================== +Ported the SimplexNoise algorithm from the C++ versions mentioned below +to a reuseable Arduino Library. + +By Jordan Shaw / http://jordanshaw.com / 2017-02 + +A C++ port of a speed-improved simplex noise algorithm for 2D in Java. +Based on example code by Stefan Gustavson (stegu@itn.liu.se). +Optimisations by Peter Eastman (peastman@drizzle.stanford.edu). +Better rank ordering method by Stefan Gustavson in 2012. +C++ port and minor type and algorithm changes by Josh Koch (jdk1337@gmail.com). +This could be speeded up even further, but it's useful as it is. +Version 2012-04-12 +The original Java code was placed in the public domain by its original author, +Stefan Gustavson. You may use it as you see fit, +but attribution is appreciated. + +Original gist url: https://gist.github.com/Slipyx/2372043 + +=============================================================================== +*/ + +#ifndef SimplexNoise_h +#define SimplexNoise_h + +#include +#include "Arduino.h" + +class Grad { +public: + Grad( int8_t x, int8_t y, int8_t z ) : x(x), y(y), z(z) {} + int8_t x, y, z; +}; + +class SimplexNoise { +public: + // Initialize permutation arrays + static void init(); + // 2D simplex noise + static double noise( double xin, double yin ); + +private: + static int32_t fastFloor( double x ); + static double dot( const Grad& g, double x, double y ); + static const double F2; + static const double G2; + static const Grad grad3[12]; + static const uint8_t p[256]; + static uint8_t perm[512]; + static uint8_t permMod12[512]; +}; + +#endif \ No newline at end of file diff --git a/prosthesis_controller/behaviour.cpp b/prosthesis_controller/behaviour.cpp new file mode 100644 index 0000000..f8c55bc --- /dev/null +++ b/prosthesis_controller/behaviour.cpp @@ -0,0 +1,186 @@ +#include "Arduino.h" +#include "behaviour.h" + +SimplexNoise sn; + + +Mood mood; + +float matrix_weights_main[SERVO_COUNT_MAIN][WEIGHT_COUNT]; //mapping outputs, sensors/moods +float matrix_weights_peripheralL[SERVO_COUNT_PERIPHERAL][WEIGHT_COUNT]; +float matrix_weights_peripheralR[SERVO_COUNT_PERIPHERAL][WEIGHT_COUNT]; + +void updateMatrixWeights(unsigned long millis, bool body_present,bool contact_main){ + mood.loneliness=1.0; + + static unsigned long last_weights_update=0; + if (millis-last_weights_update>WEIGHT_UPDATE_INTERVAL) { + last_weights_update=millis; + + static float contact_main_persist=0; + if (contact_main) { + contact_main_persist=1.0; + }else{ + if (contact_main_persist>0) { + contact_main_persist-=0.2*WEIGHT_UPDATE_INTERVAL/1000.0; + }else{ + contact_main_persist=0; + } + } + + //0=pitch + //1=roll + + + //float map_mode=constrain(mapfloat(mood.wakefulness,0,0.2, 1,0),0,1); //0=pitchroll control, 1=noise + //float map_mode=constrain(mapfloat(mood.shakiness,0,0.2, 1,0),0,1); //0=pitchroll control, 1=noise + + /*float map_mode_noise=constrain(mapfloat(mood.wakefulness,0,0.2, 0,1),0,1); + float map_mode_rollpitch=constrain(mapfloat(mood.shakiness,0,0.5, 0,1),0,1); + float map_mode_slither=0; + float map_mode_sleeping=mood.loneliness;*/ + + + bool state_bellyup=false; + bool state_map_mode_detached=false; + bool state_map_mode_attached=false; + + float map_mode_rollpitch=0; + float map_mode_slither=0; + float map_mode_sleeping=0; + + + float contact_main_smooth=constrain(mapfloat(contact_main_persist,0.0,0.3, 0.0,1.0), 0.0,1.0); + float map_mode_noise=1.0-contact_main_smooth; //idle movement + float map_mode_scared=0.0+contact_main_smooth; //contracted position + + + + + + + float map_sum=map_mode_noise+map_mode_scared; + map_mode_noise/=map_sum; //divide mapping so the sum will be 1.0 + map_mode_scared/=map_sum; + + /* + Serial.print(map_mode_noise); Serial.print(","); + Serial.print(map_mode_rollpitch); Serial.print(","); + Serial.print(map_mode_slither); Serial.print(","); + Serial.print(map_mode_sleeping); Serial.println(); + */ + + //Mode Noise + matrix_weights_main[0][W_NOISE]=30.0 *map_mode_noise; + matrix_weights_main[0][W_NOISESLOW]=0.0 *map_mode_noise; + + matrix_weights_main[1][W_NOISE]=30.0 *map_mode_noise; + matrix_weights_main[1][W_NOISESLOW]=20.0 *map_mode_noise; + + matrix_weights_main[2][W_NOISE]=20.0 *map_mode_noise; + matrix_weights_main[2][W_NOISESLOW]=30.0 *map_mode_noise; + + matrix_weights_main[0][W_LOOKDIRECTION]=30* map_mode_scared; + matrix_weights_main[1][W_LOOKDIRECTION]=30.0* map_mode_scared; + matrix_weights_main[2][W_LOOKDIRECTION]=30.0* map_mode_scared; + + + //Mode Roll Pitch + matrix_weights_main[0][W_ROLL]=1.0* map_mode_rollpitch; + matrix_weights_main[1][W_PITCH]=1.0* map_mode_rollpitch; + matrix_weights_main[2][W_ROLL]=1.0* map_mode_rollpitch; + + //Mode Slither + matrix_weights_main[0][W_COS]=30 *map_mode_slither; + matrix_weights_main[1][W_SIN]=30 *map_mode_slither; + matrix_weights_main[2][W_COS]=-30 *map_mode_slither; + + //Mode Sleeping + matrix_weights_main[0][W_NOISESLOW]=20 *map_mode_sleeping; + matrix_weights_main[1][W_NOISESLOW]=20 *map_mode_sleeping; + matrix_weights_main[2][W_NOISESLOW]=30 *map_mode_sleeping; + + /* + float sleeping=constrain(mapfloat(mood.wakefulness,0.005,0.1, 0,1),0,1); + for(int i = 0; i < SERVO_COUNT ; i++){ + matrix_weights[i][W_PITCH]*=sleeping; + matrix_weights[i][W_ROLL]*=sleeping; + matrix_weights[i][W_NOISE]*=sleeping; + matrix_weights[i][W_NOISESLOW]*=constrain(sleeping,0.1,1); + } + */ + } +} + + +void updateServosByWeights(unsigned long millis){ + static unsigned long last_servoweights_update=0; + if (millis-last_servoweights_update>SERVOWEIGHT_UPDATE_INTERVAL) + { + last_servoweights_update=millis; + static unsigned long millis_add; + #define MILLISADD_MAX 100 + millis_add+=constrain(mapfloat(mood.shakiness, 0.05,1 ,0,MILLISADD_MAX),0,MILLISADD_MAX); + + float pitch=0; + float roll=0; + + float vsin=sin((millis()+millis_add)/1000.0); + float vcos=cos((millis()+millis_add)/1000.0); + + for (uint8_t i=0;i=0)?1:-1; //if angle positive, 1, else 0 + + + + + if (i BODYTEMP_READ_MIN_INTERVAL) + { + last_read_bodytemp = millis; + + bodytemp_ambient = mlx.readAmbientTempC(); + bodytemp_object = mlx.readObjectTempC(); + + static unsigned long last_change_bodypresent=0; + if (millis-last_change_bodypresent > bodytemp_deacitvate_mintime) {} + if (!body_present) { //is currently not present + if (bodytemp_object >= bodytemp_activate_temperature && (bodytemp_object-bodytemp_ambient) >= bodytemp_activate_mindiff_ambient) { + body_present=true; + last_change_bodypresent=millis; + } + }else{ //was present last time + if((bodytemp_object-bodytemp_ambient) <= bodytemp_deactivate_maxdiff_ambient) { + body_present=false; + last_change_bodypresent=millis; + } + } + } + return body_present; +} \ No newline at end of file diff --git a/prosthesis_controller/bodytemp.h b/prosthesis_controller/bodytemp.h new file mode 100644 index 0000000..f382dbd --- /dev/null +++ b/prosthesis_controller/bodytemp.h @@ -0,0 +1,13 @@ +#ifndef BODYTEMP_H_ +#define BODYTEMP_H_ + +#include //Adafruit MLX90614 Library by Adafruit 2.1.5 +#include "definitions.h" + +#define BODYTEMP_READ_MIN_INTERVAL 500 + +void initBodytemp(); +void printBodytempDebug() ; +bool checkBodypresence(unsigned long millis); + +#endif \ No newline at end of file diff --git a/prosthesis_controller/definitions.h b/prosthesis_controller/definitions.h index 762fd8d..3e44085 100644 --- a/prosthesis_controller/definitions.h +++ b/prosthesis_controller/definitions.h @@ -22,7 +22,7 @@ #define PIN_OUT7 8 //PWM #define PIN_OUT8 9 //PWM -#define PIN_BUTTON 23 +#define PIN_BUTTON BOARD_BUTTON_PIN //BOARD_BUTTON_PIN = 23 #define PIN_PUMP1 PIN_OUT5 #define PIN_PUMP2 PIN_OUT7 @@ -32,6 +32,11 @@ #define PIN_VIBRATION PIN_OUT2 +#define PIN_SDA 25 +#define PIN_SCL 24 + +#define PIN_LED BOARD_LED_PIN //BOARD_LED_PIN=14 + #endif \ No newline at end of file diff --git a/prosthesis_controller/prosthesis_controller.ino b/prosthesis_controller/prosthesis_controller.ino index 286da12..ad663b3 100644 --- a/prosthesis_controller/prosthesis_controller.ino +++ b/prosthesis_controller/prosthesis_controller.ino @@ -1,6 +1,8 @@ #include "definitions.h" #include "vacuum.h" #include "servo.h" +#include "bodytemp.h" +#include "behaviour.h" void setup() { Serial.begin(115200); @@ -9,11 +11,16 @@ void setup() { analogWrite(PIN_VIBRATION,0); pinMode(PIN_BUTTON,INPUT_PULLDOWN); + pinMode(PIN_LED,OUTPUT); + digitalWrite(PIN_LED,HIGH); //LOW=Light, HIGH=Off initVacuum(); + initBodytemp(); pinMode(PIN_INPUTARM1,INPUT); + pinMode(PIN_INPUTARM2,INPUT); + pinMode(PIN_INPUTARM3,INPUT); pinMode(PIN_INPUTAUX,INPUT); @@ -23,20 +30,34 @@ void setup() { - - - void loop() { unsigned long loopmillis=millis(); - - bool vac=digitalRead(PIN_BUTTON); //Temporary Vacuum Button - setVacuum(vac); + + + bool body_present=checkBodypresence(loopmillis); + digitalWrite(PIN_LED,!body_present); loopVacuum(loopmillis); + + bool buttonstate=digitalRead(PIN_BUTTON); //Temporary Vacuum Button. HIGH=Pressed + if (buttonstate) { //Button pressed + setVacuum(false); //release + }else{ + setVacuum(body_present); + } + + bool contact_main=digitalRead(PIN_INPUTARM1); //Sensor on tip of arm + + updateMatrixWeights(loopmillis,body_present,contact_main); + updateServosByWeights(loopmillis); + loopServos(loopmillis); + + + //Print Debug Information static unsigned long last_print=0; if (loopmillis - last_print >100) { @@ -45,9 +66,11 @@ void loop() { //Serial.print(" Vac="); Serial.print(vac); printServoDebug(); - - Serial.println(); + //printBodytempDebug(); + + + Serial.println(); } diff --git a/prosthesis_controller/servo.cpp b/prosthesis_controller/servo.cpp index 4a6909f..0011da7 100644 --- a/prosthesis_controller/servo.cpp +++ b/prosthesis_controller/servo.cpp @@ -3,10 +3,10 @@ using namespace DYNAMIXEL; -#define SERVO_COUNT 3 const uint8_t DXL_ID = 1; const float DXL_PROTOCOL_VERSION = 2.0; + dxl_servo servos[SERVO_COUNT]; Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN); @@ -100,18 +100,15 @@ void initServos() void loopServos(unsigned long millis) { - servos[0].angle=sin(millis/1000.0/1.0)*20; + /*servos[0].angle=sin(millis/1000.0/1.0)*20; servos[1].angle=cos(millis/1000.0/1.0)*20; - servos[2].angle=sin(millis/1000.0/1.0)*20; + servos[2].angle=sin(millis/1000.0/1.0)*20;*/ static uint8_t servoUpdateI=0; //only one servo per function call static unsigned long last_servo_update=0; if (millis-last_servo_update>SERVO_UPDATE_INTERVAL/SERVO_COUNT){ last_servo_update=millis; - //for (uint8_t i=0;i "); Serial.print(servos[i].offset_angle+servos[i].angle); + //Serial.print("Servo"); Serial.print(i);Serial.print(" : "); Serial.print(dxl.getPresentPosition(servos[i].id, UNIT_DEGREE)); Serial.print(" -> "); Serial.print(servos[i].offset_angle+servos[i].angle); + Serial.print("Servo"); Serial.print(i);Serial.print(" : "); Serial.print(servos[i].offset_angle+servos[i].angle); Serial.println(); } } diff --git a/prosthesis_controller/servo.h b/prosthesis_controller/servo.h index fdd4689..670b893 100644 --- a/prosthesis_controller/servo.h +++ b/prosthesis_controller/servo.h @@ -18,6 +18,11 @@ #define SERVO_UPDATE_INTERVAL 1000/50 +#define SERVO_COUNT 3 + + + + struct dxl_servo { int id; //int initial_speed; @@ -31,6 +36,8 @@ struct dxl_servo { uint16_t modelnumber; }; + + void scanDynamixel(int32_t baud, uint8_t protocol,uint8_t maxid); void scanDynamixel(); void changeID(uint8_t present_id, uint8_t new_id);