Merge branch 'master' of https://gitlab.iri.upc.edu/humanoides/cm510_controller_fw

communications/src/serial_console.c

@@ -147,9 +147,6 @@ uint8_t serial_console_get_num_data(void)
 
 int cm510_printf(const char *fmt, ...)
 {
-  #ifdef __REAL__
-  return 0;
-  #else
   va_list ap;
   int i;
 
@@ -157,7 +154,6 @@ int cm510_printf(const char *fmt, ...)
   i = vfprintf(stdout, fmt, ap);
   va_end(ap);
   return i;
-  #endif
 }
 
 int cm510_scanf(const char *fmt, ...)

controllers/include/cm510_cfg.h

@@ -40,6 +40,6 @@
 
 // expansion board configuration parameters
 #define EXP_BOARD_USE_LOOP                      1
-#define EXP_BOARD_ID                            192
+#define EXP_BOARD_ID                            30
 
 #endif

dyn_devices/src/exp_board/exp_board.c

@@ -26,7 +26,7 @@
 #endif
 
 #ifndef EXP_BOARD_ID
-  #define EXP_BOARD_ID           192
+  #define EXP_BOARD_ID           30
 #endif
 
 uint8_t exp_board_id;

motion/src/motion_manager.c

@@ -185,14 +185,13 @@ uint8_t manager_init(uint8_t num_servos)
     manager_servos[i].center_value=0;
     manager_servos[i].current_value=0;
     manager_servos[i].module=MM_NONE;
-    manager_servos[i].slope=32;
+    manager_servos[i].slope=5;
     manager_servos[i].cw_limit=0;
     manager_servos[i].ccw_limit=0;
   }
 
   /* scan the bus for all available servos */
   dyn_master_scan(&num,servo_ids);
-
   manager_num_servos=0;
   for(i=0;i<num;i++)
   {
@@ -208,20 +207,20 @@ uint8_t manager_init(uint8_t num_servos)
       manager_servos[id].current_value=get_current_position(id);
       get_angle_limits(id,&manager_servos[i].cw_limit,&manager_servos[id].ccw_limit);
       manager_servos[id].module=MM_ACTION;
-      manager_servos[id].slope=32;
+      manager_servos[id].slope=5;
       manager_num_servos++;
     }
     else if(model==0x0C00)// Emmulated servos from the expansion board pan&tilt servos
     {
       enable_servo(id);
-      manager_servos[id].angle=300;
+      manager_servos[id].angle=180;
       manager_servos[id].max_value=1023;
       manager_servos[id].min_value=0;
       manager_servos[id].center_value=512;
       manager_servos[id].current_value=get_current_position(id);
       get_angle_limits(id,&manager_servos[i].cw_limit,&manager_servos[id].ccw_limit);
       manager_servos[id].module=MM_JOINTS;
-      manager_servos[id].slope=32;
+      manager_servos[id].slope=5;
       manager_num_servos++;
     }
   } 

motion/src/pan_tilt.c

@@ -60,14 +60,16 @@ void pan_move_angle(int8_t pan_angle)
     }
     else
     {
-      pan_current_angle=manager_servos[pan_servo_id].current_value<<7;
-      pan_target_angle=((uint32_t)(((int32_t)pan_angle*(int32_t)manager_servos[pan_servo_id].max_value)/((int32_t)manager_servos[pan_servo_id].angle))+(uint32_t)manager_servos[pan_servo_id].center_value);
+      pan_target_angle=(uint32_t)((((int32_t)pan_angle*(int32_t)manager_servos[pan_servo_id].max_value)/((int32_t)manager_servos[pan_servo_id].angle)+(int32_t)manager_servos[pan_servo_id].center_value));
       if(pan_target_angle>manager_servos[pan_servo_id].ccw_limit)
         pan_target_angle=manager_servos[pan_servo_id].ccw_limit;
       else if(pan_target_angle<manager_servos[pan_servo_id].cw_limit)
         pan_target_angle=manager_servos[pan_servo_id].cw_limit;
-      pan_target_angle=pan_target_angle<<7;
-      pan_moving=0x01;
+      if(manager_servos[pan_servo_id].current_value==pan_target_angle)
+        pan_moving=0x00;
+      else  
+        pan_moving=0x01;
+      pan_target_angle=(pan_target_angle<<7);
     }
   }
 }
@@ -99,14 +101,16 @@ void tilt_move_angle(int8_t tilt_angle)
     }
     else
     {
-      tilt_current_angle=manager_servos[tilt_servo_id].current_value<<7;
-      tilt_target_angle=(((int32_t)tilt_angle*(int32_t)manager_servos[tilt_servo_id].max_value)/((int32_t)manager_servos[tilt_servo_id].angle))+manager_servos[tilt_servo_id].center_value;
+      tilt_target_angle=(int32_t)((((int32_t)tilt_angle*(int32_t)manager_servos[tilt_servo_id].max_value)/((int32_t)manager_servos[tilt_servo_id].angle)+(int32_t)manager_servos[tilt_servo_id].center_value));
       if(tilt_target_angle>manager_servos[tilt_servo_id].ccw_limit)
         tilt_target_angle=manager_servos[tilt_servo_id].ccw_limit;
       else if(tilt_target_angle<manager_servos[tilt_servo_id].cw_limit)
         tilt_target_angle=manager_servos[tilt_servo_id].cw_limit;
-      tilt_target_angle=tilt_target_angle<<7;
-      tilt_moving=0x01;
+      if(manager_servos[tilt_servo_id].current_value==tilt_target_angle)
+        tilt_moving=0x00;
+      else
+        tilt_moving=0x01;
+      tilt_target_angle=(tilt_target_angle<<7);
     }
   }
 }
@@ -161,7 +165,7 @@ void pan_tilt_process(void)
         if(pan_current_angle<=pan_target_angle)
         {
           pan_current_angle=pan_target_angle;
-         pan_moving=0x00;
+          pan_moving=0x00;
         }
       }
     }
@@ -203,7 +207,7 @@ void pan_tilt_process(void)
           tilt_moving=0x00;
         }  
       }
-    }  
+    } 
   }
   manager_servos[tilt_servo_id].current_value=(tilt_current_angle>>7);
 }