implement more mood control

protesis_03_dxl/protesis_03_dxl.ino

@@ -41,7 +41,7 @@ void print_mpu_gyroscope_data();
 void print_mpu_offset();
 void print_loop_time_micros();
 void print_servo_torques();
-
+void readMPU();
 
 
 void setup() {
@@ -88,6 +88,7 @@ void loop() {
     //move_protesis_with_sensor_data(); //old movement function
     //slither();
 
+    readMPU();
     update_mood();
     move_protesis_organic();
     last_servo_send_millis=millis();
@@ -97,7 +98,7 @@ void loop() {
 
   if (loopmillis-last_loop_readpos_millis>100 && loopmillis-last_servo_send_millis>=4) { //limit frequency and minimum delay after send positions
     last_loop_readpos_millis=loopmillis;
-    read_servos_positions(); //takes around 8000us for all servos. read and send over serial.
+    //read_servos_positions(); //takes around 8000us for all servos. read and send over serial.
   }
   
 
@@ -110,12 +111,24 @@ void update_mood() {
   #define GYROSCOPE_FILTER_WAKEFULNESS 0.99
   mood.wakefulness = constrain(mood.wakefulness*GYROSCOPE_FILTER_WAKEFULNESS + (1-GYROSCOPE_FILTER_WAKEFULNESS)*gyro, 0,1.);
   
-  /*Serial.print(gyro);
+  double roll=current_mpu_inertia.roll_acceleration;
+  double pitch=current_mpu_inertia.pitch_acceleration;
+  double rollmapped=constrain( mapfloat(abs(roll),5,10,0,1),0,1);
+  double pitchmapped=constrain( mapfloat(abs(pitch),5,10,0,1),0,1);
+  
+  #define LONELINESS_FILTER 0.99
+  mood.loneliness= constrain( mood.loneliness*LONELINESS_FILTER + (1-(rollmapped+pitchmapped)/2)*(1-LONELINESS_FILTER) ,0.0,1.0);
+
+  /*
+  Serial.print(gyro);
   Serial.print(", ");
   Serial.print(mood.shakiness);
   Serial.print(", ");
-  Serial.println(mood.wakefulness);  
+  Serial.print(mood.wakefulness);  
+  Serial.print(", ");
+  Serial.println(mood.loneliness);  
   */
+  
 
   matrix_weights_update();
 }
@@ -142,11 +155,15 @@ void move_protesis_with_sensor_data() {
 	//print_servo_torques();
 }
 
-void move_protesis_organic() {
+void readMPU() {
   read_mpu_6050_data_with_offset_substraction(current_mpu_measurement);
 	calculate_inertia(current_mpu_inertia, current_mpu_measurement);
 	correct_inertia_drift(current_mpu_inertia);
 	denoise_inertia_with_complementary_filter();
+}
+
+void move_protesis_organic() {
+  
 	//substract_gyroscope_offset(current_mpu_measurement);
 	//mpu_measurement_with_precission(current_mpu_measurement_dp, current_mpu_measurement);
 	//denoise_mpu_measurement_with_complementary_filter();

protesis_03_dxl/servo-control.cpp

@@ -30,62 +30,76 @@ void matrix_weights_update(){
   //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.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;
+
   
   
 
-  //testing on servo
-  
-  matrix_weights[0][W_COS]=30 *map_mode;
-  matrix_weights[0][W_NOISE]=90.0 *map_mode;
-  matrix_weights[0][W_NOISESLOW]=0.0 *map_mode;
-  matrix_weights[1][W_SIN]=30 *map_mode;
-  matrix_weights[1][W_NOISE]=70.0 *map_mode;
-  matrix_weights[1][W_NOISESLOW]=20.0 *map_mode;
-  matrix_weights[2][W_COS]=-30 *map_mode;
-  matrix_weights[2][W_NOISE]=50.0 *map_mode;
-  matrix_weights[2][W_NOISESLOW]=40.0 *map_mode;
-  matrix_weights[3][W_SIN]=-30 *map_mode;
-  matrix_weights[3][W_NOISE]=20.0 *map_mode;
-  matrix_weights[3][W_NOISESLOW]=70.0 *map_mode;
-  matrix_weights[4][W_COS]=30 *map_mode;
-  matrix_weights[4][W_NOISE]=0.0 *map_mode;
-  matrix_weights[4][W_NOISESLOW]=60.0 *map_mode;
 
+  float map_sum=map_mode_noise+map_mode_rollpitch+map_mode_slither+map_mode_sleeping;
+  map_mode_noise/=map_sum; //divide mapping so the sum will be 1.0
+  map_mode_rollpitch/=map_sum;
+  map_mode_slither/=map_sum;
+  map_mode_sleeping/=map_sum;
 
-  matrix_weights[0][W_ROLL]=1.0* (1-map_mode); 
-  matrix_weights[1][W_PITCH]=1.0* (1-map_mode);  
-  matrix_weights[2][W_ROLL]=1.0* (1-map_mode);  
-  matrix_weights[3][W_PITCH]=1.0* (1-map_mode);  
-  matrix_weights[4][W_ROLL]=1.0* (1-map_mode);
   
+  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[0][W_NOISE]=90.0 *map_mode_noise;
+  matrix_weights[0][W_NOISESLOW]=0.0 *map_mode_noise;
+  
+  matrix_weights[1][W_NOISE]=70.0 *map_mode_noise;
+  matrix_weights[1][W_NOISESLOW]=20.0 *map_mode_noise;
+  
+  matrix_weights[2][W_NOISE]=50.0 *map_mode_noise;
+  matrix_weights[2][W_NOISESLOW]=40.0 *map_mode_noise;
+  
+  matrix_weights[3][W_NOISE]=20.0 *map_mode_noise;
+  matrix_weights[3][W_NOISESLOW]=70.0 *map_mode_noise;
+  
+  matrix_weights[4][W_NOISE]=0.0 *map_mode_noise;
+  matrix_weights[4][W_NOISESLOW]=60.0 *map_mode_noise;
+
+  //Mode Roll Pitch
+  matrix_weights[0][W_ROLL]=1.0* map_mode_rollpitch; 
+  matrix_weights[1][W_PITCH]=1.0* map_mode_rollpitch;  
+  matrix_weights[2][W_ROLL]=1.0* map_mode_rollpitch;  
+  matrix_weights[3][W_PITCH]=1.0* map_mode_rollpitch;  
+  matrix_weights[4][W_ROLL]=1.0* map_mode_rollpitch;
+
+  //Mode Slither
+  matrix_weights[0][W_COS]=30 *map_mode_slither;
+  matrix_weights[1][W_SIN]=30 *map_mode_slither;
+  matrix_weights[2][W_COS]=-30 *map_mode_slither;
+  matrix_weights[3][W_SIN]=-30 *map_mode_slither;
+  matrix_weights[4][W_COS]=30 *map_mode_slither;
+  
+  //Mode Sleeping
+  matrix_weights[0][W_NOISESLOW]=5 *map_mode_sleeping;
+  matrix_weights[1][W_NOISESLOW]=10 *map_mode_sleeping;
+  matrix_weights[2][W_NOISESLOW]=20 *map_mode_sleeping;
+  matrix_weights[3][W_NOISESLOW]=15 *map_mode_sleeping;
+  matrix_weights[4][W_NOISESLOW]=10 *map_mode_sleeping;
 
   /*
-  matrix_weights[0][W_NOISE]=90.0*map_mode;
+  float sleeping=constrain(mapfloat(mood.wakefulness,0.005,0.1, 0,1),0,1);
-  matrix_weights[0][W_NOISESLOW]=90.0*map_mode;
-  matrix_weights[1][W_NOISE]=90.0*map_mode;
-  matrix_weights[1][W_NOISESLOW]=90.0*map_mode;
-  matrix_weights[2][W_NOISE]=90.0*map_mode;
-  matrix_weights[2][W_NOISESLOW]=90.0*map_mode;
-  matrix_weights[3][W_NOISE]=90.0*map_mode;
-  matrix_weights[3][W_NOISESLOW]=90.0*map_mode;
-  matrix_weights[4][W_NOISE]=90.0*map_mode;
-  matrix_weights[4][W_NOISESLOW]=90.0*map_mode;
-
-  
-  */
-
-  
-  float sleeping=constrain(mapfloat(mood.wakefulness,0.005,0.02, 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);
   }
-  
+  */
   
 
 	
@@ -261,7 +275,6 @@ void map_pitch_to_servos_with_initial_position_and_move(const int pitch){
 }
 
 void map_servos_by_weights_with_initial_position_and_move(const int roll, const int pitch){
-  
   static unsigned long millis_add;
   #define MILLISADD_MAX 100
   millis_add+=constrain(mapfloat(mood.shakiness, 0.05,1 ,0,MILLISADD_MAX),0,MILLISADD_MAX);

protesis_03_dxl/servo-control.hpp

@@ -19,6 +19,7 @@
 struct Mood{
   float shakiness;
   float wakefulness;
+  float loneliness;
 };
 extern Mood mood;