branche pour mathieu

BSP @ 1cfd4a2b

-Subproject commit 987c2759314ffc01f6de6722705dc251de096143
+Subproject commit 1cfd4a2bf6001fb18a671a08775f75b5d51baa3b

Standard_robot/MDK-ARM/Standard_robot.uvoptx

@@ -471,7 +471,7 @@
       <GroupNumber>5</GroupNumber>
       <FileNumber>20</FileNumber>
       <FileType>1</FileType>
-      <tvExp>0</tvExp>
+      <tvExp>1</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
       <bDave2>0</bDave2>
       <PathWithFileName>..\BSP\Device\motor.c</PathWithFileName>
@@ -483,7 +483,7 @@
       <GroupNumber>5</GroupNumber>
       <FileNumber>21</FileNumber>
       <FileType>1</FileType>
-      <tvExp>1</tvExp>
+      <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
       <bDave2>0</bDave2>
       <PathWithFileName>..\BSP\Device\list_motor.c</PathWithFileName>

Standard_robot/Src/main.c

@@ -69,8 +69,7 @@
 int16_t led_cnt;
 float target_speed;
 
-extern RC_ctrl_t rc_ctrl;
-//char buf[200];
+extern RC_ctrl_t rc_ctrl;	
 //uint16_t pwm_pulse = 1080;  // default pwm pulse width:1080~1920
 
 
@@ -144,11 +143,9 @@ int main(void)
 	
 	can_user_init(&hcan1);                   // config can filter, start can
 	pwm_init();                              // start pwm output
-	motors_init();
+	motors_all_init();
 	//PWM_ScaleAll(&htim1);									   // etalonnage du PWM entre 0 et 1 (a faire uniquement lors de l'installation des moteurs la 1ere fois) 
-	
 
-	
   /* USER CODE END 2 */
  
  
@@ -162,7 +159,7 @@ int main(void)
     /* USER CODE BEGIN 3 */
 				
 		/* scan is key be pressed down to change target speed and pwm pulse */
-    if (key_scan())
+    /*if (key_scan())
     {
 			m3508_frontleft.target += 500;
 			m3508_frontright.target += 500;
@@ -183,49 +180,73 @@ int main(void)
 //			PWM_SetAllDuty(&htim1, 0.1f, 0.1f, 0.1f, 0.1f);
 //			HAL_Delay(2500);
 //			PWM_SetAllDuty(&htim1, 0, 0, 0, 0);
-    }
+    }*/
 		
-		if (rc_ctrl.rc.sw[0] == 1)
+		if (rc_ctrl.rc.sw[0] == 2)
     {
-			m2006.target = 15;
+			m2006.target = 0;
     };
 		if (rc_ctrl.rc.sw[0] == 3)
     {
-			m2006.target += 30;
+			m2006.target = 2000;
     };
-		if (rc_ctrl.rc.sw[0] == 2)
+		if (rc_ctrl.rc.sw[0] == 1)
+    {
+			m2006.target = 5000;
+    };
+		
+		if (rc_ctrl.rc.sw[1] == 2)
+    {
+			PWM_SetAllDuty(&htim1, 0.0f, 0.0f, 0.0f, 0.0f);
+    };
+		if (rc_ctrl.rc.sw[1] == 3)
+    {
+			PWM_SetAllDuty(&htim1, 0.25f, 0.25f, 0.25f, 0.25f);
+    };
+		if (rc_ctrl.rc.sw[1] == 1)
     {
-			m2006.target += 60;
+			PWM_SetAllDuty(&htim1, 0.5f, 0.5f, 0.5f, 0.5f);
     };
 		
 
-		m3508_frontleft.target =  4*rc_ctrl.rc.ch[0] + 4*rc_ctrl.rc.ch[1] - 4*rc_ctrl.rc.ch[4];
-		m3508_frontright.target = -(4*rc_ctrl.rc.ch[0] - 4*rc_ctrl.rc.ch[1] + 4*rc_ctrl.rc.ch[4]);
-		m3508_backleft.target =   -(4*rc_ctrl.rc.ch[0] + 4*rc_ctrl.rc.ch[1] + 4*rc_ctrl.rc.ch[4]);
-		m3508_backright.target =  4*rc_ctrl.rc.ch[0] - 4*rc_ctrl.rc.ch[1] - 4*rc_ctrl.rc.ch[4];
+		m3508_frontleft.target =  -MAX_BASE_SPEED_COEFF*(rc_ctrl.rc.ch[0] + rc_ctrl.rc.ch[1] + rc_ctrl.rc.ch[4]);
+		m3508_frontright.target = MAX_BASE_SPEED_COEFF*(rc_ctrl.rc.ch[0] - rc_ctrl.rc.ch[1] - rc_ctrl.rc.ch[4]);
+		m3508_backleft.target =   MAX_BASE_SPEED_COEFF*(rc_ctrl.rc.ch[0] + rc_ctrl.rc.ch[1] - rc_ctrl.rc.ch[4]);
+		m3508_backright.target =  -MAX_BASE_SPEED_COEFF*(rc_ctrl.rc.ch[0] - rc_ctrl.rc.ch[1] + rc_ctrl.rc.ch[4]);
+		
+		gm6020_down.target = gm6020_down.target - rc_ctrl.rc.ch[3]*0.1;
+		gm6020_up.target = gm6020_up.target + rc_ctrl.rc.ch[2]*0.04;
+		
+		
+		if (gm6020_down.target > gm6020_down.MAX_POSITION){gm6020_down.target = gm6020_down.MAX_POSITION;}
+		if (gm6020_down.target < gm6020_down.MIN_POSITION){gm6020_down.target = gm6020_down.MIN_POSITION;}
+		if (gm6020_up.target > gm6020_up.MAX_POSITION){gm6020_up.target = gm6020_up.MAX_POSITION;}
+		if (gm6020_up.target < gm6020_up.MIN_POSITION){gm6020_up.target = gm6020_up.MIN_POSITION;}
+		
 
 		get_motor_data(&m3508_frontleft);
 		get_motor_data(&m3508_frontright);
 		get_motor_data(&m3508_backleft);
 		get_motor_data(&m3508_backright);
 		get_motor_data(&m2006);
-		get_motor_data(&gm6020);
+		get_motor_data(&gm6020_up);
+		get_motor_data(&gm6020_down);
 
 		/*Affichage de donnees sur le OLED*/
-//		oled_clear(Pen_Clear);
-//		oled_printf(1, 1, "GM6020");
-//		oled_printf(2, 1,"  Target: %f", gm6020.target);
-//		oled_printf(3, 1,"  Speed: %i", gm6020.info.M3508_info.speed);
-//		oled_printf(4, 1,"  Angle: %i", gm6020.info.M3508_info.angle);
-//		oled_refresh_gram();
-
 		oled_clear(Pen_Clear);
-		oled_printf(0, 1, "Manette");
-		oled_printf(1, 1,"  CH5: %i", rc_ctrl.rc.ch[4]);
+		oled_printf(1, 1, "M2006");
+		oled_printf(2, 1,"  Target: %f", gm6020_up.info.GM6020_info.angle);
+		oled_printf(3, 1,"  Speed: %i", gm6020_down.info.GM6020_info.angle);//info.M2006_info.speed
+		oled_printf(4, 1,"  Angle: %i", gm6020_up.info.GM6020_info.angle);
+		oled_refresh_gram();
+
+//		oled_clear(Pen_Clear);
+//		oled_printf(0, 1, "Manette");
+//		oled_printf(1, 1,"  CH1: %i", rc_ctrl.rc.ch[0]);
 //		oled_printf(2, 1,"  CH2: %i", rc_ctrl.rc.ch[1]);
 //		oled_printf(3, 1,"  CH3: %i", rc_ctrl.rc.ch[2]);
 //		oled_printf(4, 1,"  CH4: %i", rc_ctrl.rc.ch[3]);
-		oled_refresh_gram();
+//		oled_refresh_gram();
 		
      /* led blink */
     led_cnt ++;
@@ -240,10 +261,11 @@ int main(void)
 		set_motor_speed(&m3508_backleft);
 		set_motor_speed(&m3508_backright);
 		set_motor_speed(&m2006);
-		set_motor_speed(&gm6020);
+		set_motor_position(&gm6020_up);
+		set_motor_position(&gm6020_down);
 
     set_motor_voltage(0, &m3508_frontleft, &m3508_frontright, &m3508_backleft, &m3508_backright); //0x200
-		set_motor_voltage(1, &gm6020, NULL, &m2006, NULL); //0x1ff
+		set_motor_voltage(1, &gm6020_up, &gm6020_down, &m2006, NULL); //0x1ff
 		
 		HAL_Delay(10);	
   }