interfisch
/
hoverbrett
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

limit backward driving with gametrak

This commit is contained in:
interfisch 2019-06-20 16:57:24 +02:00
parent daa1ee12f9
commit 4a4de3789a
1 changed files with 19 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
controller/controller.ino
View File

@ -62,9 +62,12 @@ int8_t gt_horizontal=0; //0=center
uint16_t gt_length_set=1000; //set length to keep [mm]
#define GT_LENGTH_MINDIFF 10 //[mm] threshold, do not move within gt_length_set-GT_LENGTH_MINDIFF and gt_length_set+GT_LENGTH_MINDIFF
float gt_speed_p=0.7; //value to multipy difference [mm] with -> out_speed
float gt_speedbackward_p=0.7;
float gt_steer_p=2.0;
#define GT_SPEED_LIMIT 300 //maximum out_speed value +-
#define GT_SPEED_LIMIT 300 //maximum out_speed value +
#define GT_SPEEDBACKWARD_LIMIT 100//maximum out_speed value (for backward driving) -
#define GT_STEER_LIMIT 300 //maximum out_steer value +-
#define GT_LENGTH_MAXIMUMDIFFBACKWARD -200 //[mm]. if gt_length_set=1000 and GT_LENGTH_MAXIMUMDIFFBACKWARD=-200 then only drives backward if lenght is greater 800
#include <IMUGY85.h>
@ -393,13 +396,25 @@ void loop() {
motorenabled=false;
}
int16_t _gt_length_diff = gt_length-gt_length_set; //positive if needs to drive forward
if (_gt_length_diff>-GT_LENGTH_MINDIFF & _gt_length_diff<GT_LENGTH_MINDIFF){
if (_gt_length_diff>-GT_LENGTH_MINDIFF & _gt_length_diff<GT_LENGTH_MINDIFF){ //minimum difference to drive
_gt_length_diff=0; //threshold
}
set_speed = constrain((int16_t)(_gt_length_diff*gt_speed_p),-GT_SPEED_LIMIT,GT_SPEED_LIMIT);
set_steer=constrain((int16_t)(-gt_horizontal*gt_steer_p),-GT_STEER_LIMIT,GT_STEER_LIMIT); //steer positive is left //gt_horizontal left is negative
if (_gt_length_diff>0) { //needs to drive forward
set_speed = constrain((int16_t)(_gt_length_diff*gt_speed_p),0,GT_SPEED_LIMIT);
}else{ //drive backward
if (_gt_length_diff > GT_LENGTH_MAXIMUMDIFFBACKWARD){ //only drive if not pulled back too much
set_speed = constrain((int16_t)(_gt_length_diff*gt_speedbackward_p),-GT_SPEEDBACKWARD_LIMIT,0);
}else{
set_speed = 0; //stop
set_steer = 0;
}
}
//calculate speed l and r from speed and steer
#define SPEED_COEFFICIENT_GT 1 // higher value == stronger
#define STEER_COEFFICIENT_GT 0.5 // higher value == stronger