From 50d270781b2463f3b26b04496e07c15183534ea2 Mon Sep 17 00:00:00 2001
From: sfaguet <sfaguet@centumadetel.com>
Date: Mon, 22 Feb 2021 09:12:02 -0500
Subject: [PATCH] =?UTF-8?q?R=C3=A9glages=20suite=20=C3=A0=20la=20rencontre?=
=?UTF-8?q?=20pilote=20+=20deadzone?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
Inc/jetson.h | 6 +++---
Inc/pilotes.h | 2 ++
MDK-ARM/RobotMaster_allRobots.uvoptx | 4 ++--
Src/BoardA_handle.c | 2 +-
Src/motors.c | 10 ++++++++++
Src/pilotes.c | 25 +++++++++++++++----------
Src/receiver_RadioController.c | 17 -----------------
Src/traitement.c | 21 ++++++++++++---------
8 files changed, 45 insertions(+), 42 deletions(-)
diff --git a/Inc/jetson.h b/Inc/jetson.h
index a4b9374..1573524 100644
--- a/Inc/jetson.h
+++ b/Inc/jetson.h
@@ -28,9 +28,9 @@ typedef __packed struct
typedef __packed struct
{
uint8_t target_located;
- uint16_t teta_target_location;
- uint16_t phi_target_location;
- uint16_t d_target_location;
+ uint16_t teta_target_location; //mrad
+ int16_t phi_target_location; //mrad
+ uint16_t d_target_location; //mm
} robot_target_coordinates_t;
typedef __packed struct
diff --git a/Inc/pilotes.h b/Inc/pilotes.h
index e093cc2..05c0eaf 100644
--- a/Inc/pilotes.h
+++ b/Inc/pilotes.h
@@ -12,6 +12,8 @@
typedef struct
{
+ float sensitivity_RC_deadzone;
+ float sensitivity_mouse_deadzone;
float sensitivity_ch_1;
float sensitivity_ch_2;
float sensitivity_mouse_x;
diff --git a/MDK-ARM/RobotMaster_allRobots.uvoptx b/MDK-ARM/RobotMaster_allRobots.uvoptx
index 4fb4b27..f61020c 100644
--- a/MDK-ARM/RobotMaster_allRobots.uvoptx
+++ b/MDK-ARM/RobotMaster_allRobots.uvoptx
@@ -525,7 +525,7 @@
<Group>
<GroupName>Application/User</GroupName>
- <tvExp>1</tvExp>
+ <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<cbSel>0</cbSel>
<RteFlg>0</RteFlg>
@@ -693,7 +693,7 @@
<Group>
<GroupName>Devices</GroupName>
- <tvExp>1</tvExp>
+ <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<cbSel>0</cbSel>
<RteFlg>0</RteFlg>
diff --git a/Src/BoardA_handle.c b/Src/BoardA_handle.c
index 4d07305..627f612 100644
--- a/Src/BoardA_handle.c
+++ b/Src/BoardA_handle.c
@@ -106,7 +106,7 @@ void uart_debug(){
return;
}
tickstart = HAL_GetTick();
- snprintf(buff2, 1000, "%c,teta=%x,phi=%x,d=%x\r\n", jetson.robot_target_coordinates.target_located, jetson.robot_target_coordinates.teta_target_location, jetson.robot_target_coordinates.phi_target_location, jetson.robot_target_coordinates.d_target_location);
+ snprintf(buff2, 1000, "%c,teta=%d,phi=%d,d=%d \r", jetson.robot_target_coordinates.target_located, jetson.robot_target_coordinates.teta_target_location, jetson.robot_target_coordinates.phi_target_location, jetson.robot_target_coordinates.d_target_location);
HAL_UART_Transmit_DMA(&huart8, (uint8_t*)buff2, strlen(buff2));
/*
uart_debug_command("[2J"); //Clear entire screen
diff --git a/Src/motors.c b/Src/motors.c
index 342bb9b..96eddf0 100644
--- a/Src/motors.c
+++ b/Src/motors.c
@@ -7,6 +7,8 @@
/* On récupère les variables exterieurs */
extern motor_t motors[MAX_MOTORS];
+extern pilote_t pilote;
+extern receiver_RadioController_t receiver_RadioController;
/* Envoie les commandes sur le bus CAN pour tous les moteurs CAN */
void can_send_command(){
@@ -76,6 +78,14 @@ 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){
+ 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;
diff --git a/Src/pilotes.c b/Src/pilotes.c
index b6703a2..944a199 100644
--- a/Src/pilotes.c
+++ b/Src/pilotes.c
@@ -13,18 +13,23 @@ void piloteInit(uint8_t pilote_id){
Robot ID:
0: Antonin;
*/
- pilote.sensitivity_ch_1 = 0.00005;
- pilote.sensitivity_ch_2 = 0.00005;
- pilote.sensitivity_mouse_x = 0.000005;
- pilote.sensitivity_mouse_y = 0.000005;
+ /* Receiver */
+ pilote.sensitivity_RC_deadzone = 50; //Between 0 and 6600
+ pilote.sensitivity_ch_1 = 0.00003;
+ pilote.sensitivity_ch_2 = 0.00003;
- pilote.sensitivity_chassis_RC_Vx = 10;
- pilote.sensitivity_chassis_RC_Vy = 10;
- pilote.sensitivity_chassis_RC_W = 3;
+ pilote.sensitivity_chassis_RC_Vx = 20;
+ pilote.sensitivity_chassis_RC_Vy = 20;
+ pilote.sensitivity_chassis_RC_W = 6;
+
+ /* Mouse Keyboard */
+ pilote.sensitivity_mouse_deadzone = 50; //Between 0 and 6600
+ pilote.sensitivity_mouse_x = 0.0005;
+ pilote.sensitivity_mouse_y = 0.0005;
- pilote.sensitivity_chassis_keyboard_Vx = 1000;
- pilote.sensitivity_chassis_keyboard_Vy = 1000;
- pilote.sensitivity_chassis_mouse_W = 5;
+ pilote.sensitivity_chassis_keyboard_Vx = 4300;
+ pilote.sensitivity_chassis_keyboard_Vy = 4300;
+ pilote.sensitivity_chassis_mouse_W = 160;
switch(pilote_id){ //Configuration personnalisée
/* Antonin */
diff --git a/Src/receiver_RadioController.c b/Src/receiver_RadioController.c
index 114914a..b85be7e 100644
--- a/Src/receiver_RadioController.c
+++ b/Src/receiver_RadioController.c
@@ -29,23 +29,6 @@ void receiver_RadioController_callback_handler()
receiver_RadioController.data.ch4 = (uart1_rx_buff[4] >> 1 | uart1_rx_buff[5] << 7) & 0x07FF;
receiver_RadioController.data.ch4 -= 1024;
- /* prevent remote control zero deviation */
- if (receiver_RadioController.data.ch1 <= 5 && receiver_RadioController.data.ch1 >= -5)
- {
- receiver_RadioController.data.ch1 = 0;
- }
- if (receiver_RadioController.data.ch2 <= 5 && receiver_RadioController.data.ch2 >= -5)
- {
- receiver_RadioController.data.ch2 = 0;
- }
- if (receiver_RadioController.data.ch3 <= 5 && receiver_RadioController.data.ch3 >= -5)
- {
- receiver_RadioController.data.ch3 = 0;
- }
- if (receiver_RadioController.data.ch4 <= 5 && receiver_RadioController.data.ch4 >= -5)
- {
- receiver_RadioController.data.ch4 = 0;
- }
receiver_RadioController.data.ch1_float = (float) (receiver_RadioController.data.ch1 / 6.6); //Entre -100% et 100%
receiver_RadioController.data.ch2_float = (float) (receiver_RadioController.data.ch2 / 6.6);
receiver_RadioController.data.ch3_float = (float) (receiver_RadioController.data.ch3 / 6.6);
diff --git a/Src/traitement.c b/Src/traitement.c
index 32919b2..44a7607 100644
--- a/Src/traitement.c
+++ b/Src/traitement.c
@@ -30,7 +30,7 @@ void traitement_1(){
if(receiver_RadioController.keyboard_mode){
double chassis_w;
double tourelle_yaw;
- if(receiver_RadioController.data.kb.bit.CTRL){
+ if(!receiver_RadioController.data.kb.bit.CTRL){
chassis_w = -receiver_RadioController.data.mouse.x;
tourelle_yaw = 0;
}else{
@@ -46,9 +46,9 @@ void traitement_1(){
if(receiver_RadioController.data.mouse.l){
- canon_shoot(0.15, 2000);
+ canon_shoot(0.15, 1000);
}else if(receiver_RadioController.data.mouse.r){
- canon_shoot(0.20, 5000);
+ canon_shoot(0.20, 1000);
}else{
canon_shoot_end();
}
@@ -58,24 +58,21 @@ void traitement_1(){
switch(receiver_RadioController.data.sw1){
case 1:
- motors[FEEDER].consigne = 0;
break;
case 3:
- motors[FEEDER].consigne = 5000;
break;
case 2:
- motors[FEEDER].consigne = 10000;
break;
}
switch(receiver_RadioController.data.sw2){
case 1:
- PWM_SetAllDuty(&htim1, 0.0, 0.0);
+ canon_shoot(0, 0);
break;
case 3:
- PWM_SetAllDuty(&htim1, 0.25, 0.25);
+ //canon_shoot(0.40, 1000);
break;
case 2:
- PWM_SetAllDuty(&htim1, 0.50, 0.50);
+ //canon_shoot(1, 1000);
break;
}
@@ -92,15 +89,21 @@ void chassis_consigne(double Vx, double Vy, double W){
double sensitivity_Vx;
double sensitivity_Vy;
double sensitivity_W;
+ double sensitivity_deadzone;
if(receiver_RadioController.keyboard_mode){
sensitivity_Vx = pilote.sensitivity_chassis_keyboard_Vx;
sensitivity_Vy = pilote.sensitivity_chassis_keyboard_Vy;
sensitivity_W = pilote.sensitivity_chassis_mouse_W;
+ sensitivity_deadzone = 0;
}else{
sensitivity_Vx = pilote.sensitivity_chassis_RC_Vx;
sensitivity_Vy = pilote.sensitivity_chassis_RC_Vy;
sensitivity_W = pilote.sensitivity_chassis_RC_W;
+ sensitivity_deadzone = pilote.sensitivity_RC_deadzone;
}
+ if(Vx > -sensitivity_deadzone && Vx < sensitivity_deadzone) Vx = 0;
+ if(Vy > -sensitivity_deadzone && Vy < sensitivity_deadzone) Vy = 0;
+
motors[FRONT_LEFT].consigne = sensitivity_Vx*Vx - sensitivity_Vy*Vy - sensitivity_W*W;
motors[FRONT_RIGHT].consigne = -(sensitivity_Vx*Vx + sensitivity_Vy*Vy + sensitivity_W*W);
motors[BACK_RIGHT].consigne = -(sensitivity_Vx*Vx - sensitivity_Vy*Vy + sensitivity_W*W);
--
GitLab