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/Src/motors.c b/Src/motors.c
index 96eddf0ece6d00d233d99654b091c58444343a05..0ad2e46676e80c1aca135a237e0162aae7127c15 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,19 +56,19 @@ 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:
+ //Si c'est le premier paquet, aller le lire et initialiser la tourelle
+ if(motors[j].signOfLife_tick == 0) {
+ fill_motor_data(&motors[j], rx_buff);
+ init_tourelle_data(&motors[j]);
+ break;
+ }
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]);
+ fill_motor_data(&motors[j], rx_buff);
break;
}
return;
@@ -76,22 +76,45 @@ void can_motors_callback_handler(int16_t rx_id, uint8_t* rx_buff){
}
}
+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){
- double sensitivity_deadzone;
- if(receiver_RadioController.keyboard_mode){
- sensitivity_deadzone = pilote.sensitivity_mouse_deadzone;
- }else{
- sensitivity_deadzone = pilote.sensitivity_RC_deadzone;
+ //S'assure que le moteur répond avant d'envoyer des consignes
+ if (motor->signOfLife_tick != 0 && HAL_GetTick() - motor->signOfLife_tick < 100){
+ double sensitivity_deadzone;
+ if(receiver_RadioController.keyboard_mode){
+ sensitivity_deadzone = pilote.sensitivity_mouse_deadzone;
+ }else{
+ sensitivity_deadzone = pilote.sensitivity_RC_deadzone;
+ }
+ if(value > -sensitivity_deadzone && value < sensitivity_deadzone) value = 0;
+
+ float consigne_position = motor->consigne + (motor->direction * value * coeff);
+ if(consigne_position > 360) consigne_position -= (float) 360.0;
+ if(consigne_position < 0) consigne_position += (float) 360.0;
+ if(motor->MAX_POSITION > 0 && consigne_position > motor->MAX_POSITION) consigne_position = motor->MAX_POSITION;
+ if(motor->MIN_POSITION > 0 && consigne_position < motor->MIN_POSITION) consigne_position = motor->MIN_POSITION;
+ motor->consigne = consigne_position;
}
- if(value > -sensitivity_deadzone && value < sensitivity_deadzone) value = 0;
-
- float consigne_position = motor->consigne + (motor->direction * value * coeff);
- if(consigne_position > 360) consigne_position -= (float) 360.0;
- if(consigne_position < 0) consigne_position += (float) 360.0;
- if(motor->MAX_POSITION > 0 && consigne_position > motor->MAX_POSITION) consigne_position = motor->MAX_POSITION;
- if(motor->MIN_POSITION > 0 && consigne_position < motor->MIN_POSITION) consigne_position = motor->MIN_POSITION;
- motor->consigne = consigne_position;
}
/* Initialise le CAN 1 */
diff --git a/Src/robot_configuration.c b/Src/robot_configuration.c
index 616e32a5a697587ac0e214b29fcdf704086ddf19..0c9c504d8c6031cfaede9677518ab020dd83459b 100644
--- a/Src/robot_configuration.c
+++ b/Src/robot_configuration.c
@@ -114,9 +114,9 @@ void robotInit(uint8_t robot_id){
motors[TOURELLE_PITCH].can_rx_id = 0x204+1; // ID = 1
motors[TOURELLE_PITCH].can_tx_frame = 0x1FF;
motors[TOURELLE_PITCH].can_tx_id = 1;
- motors[TOURELLE_PITCH].MIN_POSITION = 1; //en deg
- motors[TOURELLE_PITCH].MAX_POSITION = 67; //en deg //213 120
- motors[TOURELLE_PITCH].consigne = 54; //en deg //Valeur initiale
+ motors[TOURELLE_PITCH].MIN_POSITION = 243; //en deg
+ motors[TOURELLE_PITCH].MAX_POSITION = 293; //en deg //213 120
+ motors[TOURELLE_PITCH].consigne = 260; //en deg //Valeur initiale
motors[TOURELLE_PITCH].direction = -1; //permet de choisir la direction de controle (-1 ou 1)
pid_create(&motors[TOURELLE_PITCH].pid,
&motors[TOURELLE_PITCH].info.angle_360, //input : le retour sur la quelle ont veut atteintre la consigne
@@ -131,9 +131,9 @@ void robotInit(uint8_t robot_id){
motors[TOURELLE_YAW].can_rx_id = 0x204+2; // ID = 2
motors[TOURELLE_YAW].can_tx_frame = 0x1FF;
motors[TOURELLE_YAW].can_tx_id = 2;
- motors[TOURELLE_YAW].MIN_POSITION = 245; //en deg
- motors[TOURELLE_YAW].MAX_POSITION = 359; //en deg
- motors[TOURELLE_YAW].consigne = 325; //en deg //Valeur initiale
+ motors[TOURELLE_YAW].MIN_POSITION = 155; //en deg
+ motors[TOURELLE_YAW].MAX_POSITION = 269; //en deg
+ motors[TOURELLE_YAW].consigne = 215; //en deg //Valeur initiale
motors[TOURELLE_YAW].direction = -1; //permet de choisir la direction de controle (-1 ou 1)
pid_create(&motors[TOURELLE_YAW].pid,
&motors[TOURELLE_YAW].info.angle_360, //input : le retour sur la quelle ont veut atteintre la consigne