diff --git a/Inc/motors.h b/Inc/motors.h
index d1a21995d839e5bdad9a20d87dc7f364c3bb755c..735bd6f43f13d105c4a29925a8c4dc092244a96b 100644
--- a/Inc/motors.h
+++ b/Inc/motors.h
@@ -65,6 +65,9 @@ void can_motors_callback_handler(int16_t rx_id, uint8_t* rx_buff);
/* Modifie la consigne tout en vérifiant les limites de postion */
void add_consigne_position(motor_t* motor, float value, float coeff);
+/* Initialise la tourelle aux valeurs de départ des moteurs */
+void init_tourelle_data(motor_t* motor);
+
/* Initialise le CAN 1 */
void can1_init(void);
@@ -77,4 +80,7 @@ void PWM_SetAllDuty(TIM_HandleTypeDef *tim, float duty_ch1, float duty_ch2);
/* scales all PWM duty cycles between 0 and 1 */
void PWM_ScaleAll(TIM_HandleTypeDef *tim, bool switchRotationalDirection);
+/*Rempli la structure motor.info avec les données provenant du moteur */
+void fill_motor_data (motor_t* motor, uint8_t* rx_buff);
+
#endif
diff --git a/Inc/traitement.h b/Inc/traitement.h
index 83754f07a67655dcae2339088b1a77845221f7ca..936f987c7aebf12b2bb4d6809ada92d02fa774f5 100644
--- a/Inc/traitement.h
+++ b/Inc/traitement.h
@@ -36,4 +36,7 @@ void chassis_consigne(double Vx, double Vy, double W);
/*Effectue le suivi automatique des cibles */
void auto_follow_target(void);
+/*Retourne true si le controller est a une position neutre*/
+bool isControllerNeutral(void);
+
#endif
diff --git a/MDK-ARM/RobotMaster_allRobots.uvoptx b/MDK-ARM/RobotMaster_allRobots.uvoptx
index e9f12221a83f91e0c8f2a833f6f3e2e1378126fa..0f4216ea5d3aebadad780ea0bafe187f8fe5b20f 100644
--- a/MDK-ARM/RobotMaster_allRobots.uvoptx
+++ b/MDK-ARM/RobotMaster_allRobots.uvoptx
@@ -690,7 +690,7 @@
<Group>
<GroupName>BoardA</GroupName>
- <tvExp>0</tvExp>
+ <tvExp>1</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<cbSel>0</cbSel>
<RteFlg>0</RteFlg>
@@ -710,7 +710,7 @@
<Group>
<GroupName>Devices</GroupName>
- <tvExp>0</tvExp>
+ <tvExp>1</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<cbSel>0</cbSel>
<RteFlg>0</RteFlg>
@@ -814,7 +814,7 @@
<Group>
<GroupName>Traitement</GroupName>
- <tvExp>0</tvExp>
+ <tvExp>1</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<cbSel>0</cbSel>
<RteFlg>0</RteFlg>
diff --git a/MDK-ARM/RobotMaster_allRobots.uvprojx b/MDK-ARM/RobotMaster_allRobots.uvprojx
index 1a5ae0cb6e49d68f2b0acfe90122a657c8b88d19..18e7605bcd43052e2aacb29d180a45aac56e15d0 100644
--- a/MDK-ARM/RobotMaster_allRobots.uvprojx
+++ b/MDK-ARM/RobotMaster_allRobots.uvprojx
@@ -16,7 +16,7 @@
<TargetCommonOption>
<Device>STM32F427IIHx</Device>
<Vendor>STMicroelectronics</Vendor>
- <PackID>Keil.STM32F4xx_DFP.2.15.0</PackID>
+ <PackID>Keil.STM32F4xx_DFP.2.14.0</PackID>
<PackURL>http://www.keil.com/pack/</PackURL>
<Cpu>IRAM(0x20000000,0x00030000) IRAM2(0x10000000,0x00010000) IROM(0x08000000,0x00200000) CPUTYPE("Cortex-M4") FPU2 CLOCK(12000000) ELITTLE</Cpu>
<FlashUtilSpec></FlashUtilSpec>
diff --git a/Src/main.c b/Src/main.c
index 2043d574e3395802b774d89b4c768cd9811110b4..6160fe14dc8c35f6280cf4df596307bf20179063 100644
--- a/Src/main.c
+++ b/Src/main.c
@@ -113,11 +113,21 @@ int main(void)
extern motor_t motors[MAX_MOTORS];
extern pilote_t pilote;
extern jetson_t jetson;
+ extern uint32_t signOfLife_Receiver_RadioController_tick;
piloteInit(PILOTE_ANTONIN);
/* USER CODE END 2 */
/* Infinite loop */
/* USER CODE BEGIN WHILE */
+
+ while(1){
+ if(signOfLife_Receiver_RadioController_tick !=0 ){
+ if(isControllerNeutral()){
+ break;
+ }
+ }
+ }
+
while (1)
{
signOfLife(); //LEDs BLINK
diff --git a/Src/motors.c b/Src/motors.c
index 96eddf0ece6d00d233d99654b091c58444343a05..971082dddc2691d33d7fa35f56c1671c19b4d1fb 100644
--- a/Src/motors.c
+++ b/Src/motors.c
@@ -34,8 +34,8 @@ void can_send_command(){
for(int j = 0; j < MAX_MOTORS ; j++){
if(motors[j].can_tx_frame == trams[i] && motors[j].can_tx_id == 1){
- tx_data[0] = ((int16_t) motors[j].command >> 8)&0xff;
- tx_data[1] = ((int16_t) motors[j].command )&0xff;
+ tx_data[0] = ((int16_t) motors[j].command >> 8)&0xff;
+ tx_data[1] = ((int16_t) motors[j].command )&0xff;
}
if(motors[j].can_tx_frame == trams[i] && motors[j].can_tx_id == 2){
tx_data[2] = ((int16_t) motors[j].command >> 8)&0xff;
@@ -56,28 +56,45 @@ void can_send_command(){
/* Fonction appelée lors de la réception d'une information provenant d'un moteur */
void can_motors_callback_handler(int16_t rx_id, uint8_t* rx_buff){
- int16_t speed;
for(int j = 0; j < MAX_MOTORS ; j++){
if(rx_id == motors[j].can_rx_id){
- motors[j].signOfLife_tick = HAL_GetTick();
- switch(motors[j].type){
- case GM6020:
- case M3508:
- case M2006:
- motors[j].info.angle = (int16_t)(rx_buff[0] << 8 | rx_buff[1]); //0 à 8191
- motors[j].info.angle_360 = 360.0 * motors[j].info.angle / 8191.0; //0 à 360 deg
- speed = (rx_buff[2] << 8 | rx_buff[3]);//rpm
- motors[j].info.speed = (float)speed;//rpm
- motors[j].info.torque = (int16_t)(rx_buff[4] << 8 | rx_buff[5]);
- break;
+ //Si c'est le premier paquet, aller le lire et initialiser la tourelle
+ if(motors[j].type == GM6020 && motors[j].signOfLife_tick == 0) {
+ fill_motor_data(&motors[j], rx_buff);
+ init_tourelle_data(&motors[j]);
}
+ fill_motor_data(&motors[j], rx_buff);
return;
}
}
}
+void init_tourelle_data(motor_t* motor){
+ motor->consigne = motor->info.angle_360;
+ if(motor->MAX_POSITION < motor->info.angle_360 || motor->MIN_POSITION > motor->info.angle_360){
+ while(true){
+ BOARD_LED_GREEN_ON();
+ }
+ }
+}
+
+/*Rempli la structure motor.info avec les données provenant du moteur */
+void fill_motor_data (motor_t* motor, uint8_t* rx_buff){
+ int16_t speed;
+ motor->signOfLife_tick = HAL_GetTick();
+ motor->info.angle = (int16_t)(rx_buff[0] << 8 | rx_buff[1]); //0 à 8191
+ motor->info.angle_360 = 360.0 * motor->info.angle / 8191.0; //0 à 360 deg
+ speed = (rx_buff[2] << 8 | rx_buff[3]);//rpm
+ motor->info.speed = (float)speed;//rpm
+ motor->info.torque = (int16_t)(rx_buff[4] << 8 | rx_buff[5]);
+}
+
/* Modifie la consigne tout en vérifiant les limites de postion */
void add_consigne_position(motor_t* motor, float value, float coeff){
+
+ //S'assure que le moteur répond avant d'envoyer des consignes
+ if (motor->signOfLife_tick != 0 && HAL_GetTick() - motor->signOfLife_tick < 100) return;
+
double sensitivity_deadzone;
if(receiver_RadioController.keyboard_mode){
sensitivity_deadzone = pilote.sensitivity_mouse_deadzone;
diff --git a/Src/robot_configuration.c b/Src/robot_configuration.c
index 616e32a5a697587ac0e214b29fcdf704086ddf19..3706c0238d10d630290260a7bcff5cc4d38c9b8b 100644
--- a/Src/robot_configuration.c
+++ b/Src/robot_configuration.c
@@ -8,6 +8,9 @@
/* Crée le tableau contenant tous les moteurs du robot */
motor_t motors[MAX_MOTORS];
+// Variable qui sauvegarde le type de robot
+int robot_type;
+
/* CAN MOTORS DATASHEET
M3508: Control by current
TX :
@@ -32,7 +35,6 @@ motor_t motors[MAX_MOTORS];
*/
-
/* Fonction qui premet de configurer le robot */
void robotInit(uint8_t robot_id){
/*
diff --git a/Src/traitement.c b/Src/traitement.c
index 4b8ea4ef3e1ec275326e6d7e7815f4fa69992cf9..b266c2963df2354fe0dbab952b6b65fc51f7a991 100644
--- a/Src/traitement.c
+++ b/Src/traitement.c
@@ -30,6 +30,34 @@ void traitement_pids_compute(){
}
}
+/*Returns true if controller is in neutral state*/
+bool isControllerNeutral(){
+
+ if(receiver_RadioController.data.ch1_float != 0){
+ return false;
+ }
+ if(receiver_RadioController.data.ch2_float != 0){
+ return false;
+ }
+ if(receiver_RadioController.data.ch3_float != 0){
+ return false;
+ }
+ if(receiver_RadioController.data.ch4_float != 0){
+ return false;
+ }
+ if(receiver_RadioController.data.sw1 != 1){
+ return false;
+ }
+ if(receiver_RadioController.data.sw2 != 1){
+ return false;
+ }
+ /*if(receiver_RadioController.data.wheel != 0){
+ return false;
+ }*/
+ return true;
+}
+
+
/* Fonctions qui fait les liens entre les entrées (capteurs, radio controller, CV, ...) et les sorties (consignes moteurs), on peut créer plusieurs traitements */
void traitement_1(){