add simple overload fault recovery

protesis_03_dxl/protesis_03_dxl.ino

@@ -80,17 +80,20 @@ void loop() {
 
   static unsigned long last_servo_send_millis;
 
+  
+
   if (loopmillis-last_loop_movement_millis>50) {
     last_loop_movement_millis=loopmillis;
-    //int state_switch_programm = digitalRead(switch_programm);  //currently not used
+    int state_switch_programm = digitalRead(switch_programm);  //currently not used
     //Serial.println(digitalRead(switch_programm));
+    
   
     //move_protesis_with_sensor_data(); //old movement function
     //slither();
 
     readMPU();
     update_mood();
-    move_protesis_organic();
+    map_servos_by_weights_with_initial_position_and_move(current_mpu_inertia.roll_acceleration,current_mpu_inertia.pitch_acceleration);
     last_servo_send_millis=millis();
 
     //Serial.println("moved servos");
@@ -98,7 +101,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.
   }
   
 
@@ -107,7 +110,7 @@ void loop() {
 void update_mood() {  
   #define GYROSCOPE_FILTER_SHAKINESS 0.8
   float gyro=constrain(sqrt(pow(current_mpu_measurement.gyroscope.x,2)+pow(current_mpu_measurement.gyroscope.y,2)) / 10000.0   ,0.0, 1.0);
-  mood.shakiness = constrain(mood.shakiness*GYROSCOPE_FILTER_SHAKINESS + (1-GYROSCOPE_FILTER_SHAKINESS)*gyro, 0,1.) ;
+  mood.shakiness = constrain(mood.shakiness*GYROSCOPE_FILTER_SHAKINESS + (1-GYROSCOPE_FILTER_SHAKINESS)*gyro, 0,1.);
   #define GYROSCOPE_FILTER_WAKEFULNESS 0.99
   mood.wakefulness = constrain(mood.wakefulness*GYROSCOPE_FILTER_WAKEFULNESS + (1-GYROSCOPE_FILTER_WAKEFULNESS)*gyro, 0,1.);
   
@@ -162,19 +165,6 @@ void readMPU() {
 	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();
-
-	//servos_collect_current_torque();
-  map_servos_by_weights_with_initial_position_and_move(current_mpu_inertia.roll_acceleration,current_mpu_inertia.pitch_acceleration);
-	
-
-
-}
-
 void denoise_inertia_with_complementary_filter() {
 	current_mpu_inertia.roll_gyroscope = previous_mpu_inertia.roll_gyroscope
 			* DENOISE_PREVIOUS_INERTIA_RATE

protesis_03_dxl/servo-control.cpp

@@ -48,11 +48,12 @@ void matrix_weights_update(){
   map_mode_slither/=map_sum;
   map_mode_sleeping/=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[0][W_NOISE]=90.0 *map_mode_noise;
@@ -85,9 +86,9 @@ void matrix_weights_update(){
   matrix_weights[4][W_COS]=30 *map_mode_slither;
   
   //Mode Sleeping
-  matrix_weights[0][W_NOISESLOW]=5 *map_mode_sleeping;
+  matrix_weights[0][W_NOISESLOW]=20 *map_mode_sleeping;
-  matrix_weights[1][W_NOISESLOW]=10 *map_mode_sleeping;
+  matrix_weights[1][W_NOISESLOW]=20 *map_mode_sleeping;
-  matrix_weights[2][W_NOISESLOW]=20 *map_mode_sleeping;
+  matrix_weights[2][W_NOISESLOW]=30 *map_mode_sleeping;
   matrix_weights[3][W_NOISESLOW]=15 *map_mode_sleeping;
   matrix_weights[4][W_NOISESLOW]=10 *map_mode_sleeping;
 
@@ -167,6 +168,7 @@ void servos_init_with_current_position() {
 	servos_init();
 }
 
+
 void servos_init() {
 	for(int i = 0; i< SERVO_COUNT; i++) {
     dxlSetStatusReturnLevel(servos[i].id, AX_RETURN_READ); // Incorporated to avoid the colapsing of data found it with the osciloscope when the motors are sending data to te board
@@ -314,9 +316,10 @@ void map_servos_by_weights_with_initial_position_and_move(const int roll, const
   
 }
 
-
+#define CHECK_BIT(var,pos) ((var) & (1<<(pos)))
 void read_servos_positions()
 {
+  int errors[SERVO_COUNT];
   Serial.print("dxlgp");
   for (uint8_t i=0;i<SERVO_COUNT;i++) {
     //Serial.print("servo "); Serial.print(i); Serial.print( "(id="); Serial.print(servos[i].id); Serial.print(")"); Serial.print("="); Serial.println(dxlGetPosition(servos[i].id));
@@ -324,9 +327,41 @@ void read_servos_positions()
     Serial.print(",");
     
     Serial.print(dxlGetPosition(servos[i].id)); 
+    errors[i]=ax12GetLastError();
+    
+    
     
   }
   Serial.println();
+
+  Serial.print("error");
+  for (uint8_t i=0;i<SERVO_COUNT;i++) {
+    Serial.print(",");    
+    Serial.print(errors[i]); 
+  }
+  Serial.println();
+
+  
+
+  for(int i = 0; i< SERVO_COUNT; i++) {
+    int errorcode=errors[i];
+    if (errorcode!=0) {
+      Serial.print("Error present on "); Serial.print(i); Serial.print("errorcode="); Serial.println(errorcode);
+    }
+    if (CHECK_BIT(errorcode,5)) { //overload error
+      Serial.print("Init Servo "); Serial.println(i);
+      delay(1000);
+      dxlSetStatusReturnLevel(servos[i].id, AX_RETURN_READ); // Incorporated to avoid the colapsing of data found it with the osciloscope when the motors are sending data to te board
+      dxlSetJointMode(servos[i].id, servos[i].min_position, servos[i].max_position);
+      dxlSetGoalSpeed(servos[i].id, servos[i].initial_speed);
+      dxlTorqueOn(servos[i].id);
+      dxlSetRunningTorqueLimit(servos[i].id,MOTOR_TORQUE_LIMIT / MOTOR_TORQUE_RATIO);
+      dxlSetGoalPosition(servos[i].id, servos[i].initial_position);
+
+      
+      
+    }
+  }
 }