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

implement gametrak function

This commit is contained in:
interfisch 2019-06-12 00:30:53 +02:00
parent ce466e17b1
commit 5e17c2cf22
1 changed files with 40 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

48
controller.ino
View File

@ -59,8 +59,12 @@ int8_t gt_vertical=0; //0=center. joystick can rotate +-30 degrees. -127 = -30 d
#define GT_HORIZONTAL_RANGE 2047 //adc value difference from center to maximum (30 deg)
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_steer_p=1.0;
#define GT_SPEED_LIMIT 300 //maximum out_speed value +-
#define GT_STEER_LIMIT 200 //maximum out_steer value +-
#include <IMUGY85.h>
@ -139,7 +143,8 @@ uint8_t out_checksum = 0; //0= disable motors, 255=reserved, 1<=checksum<255
boolean armed = false;
boolean lastpacketOK = false;
//Gametrak
boolean armed_gt = false;
void setup() {
@ -309,6 +314,8 @@ void loop() {
#endif
}
if (armed) { //is armed
armed_gt = false; //if remote used disable gametrak
if (lastpacketOK) { //if lastnrfdata is valid
if (millis() - last_controlupdate > CONTROLUPDATEPERIOD) {
last_controlupdate = millis();
@ -349,20 +356,45 @@ void loop() {
}//if pastpacket not ok, keep last out_steer and speed values until disarmed
#ifdef DEBUG
if (!lastpacketOK)
if (!lastpacketOK) {
Serial.println("Armed but packet not ok");
}
#endif
} else { //disarmed
out_steer = 0;
out_speed = 0;
}else{ //not armed, keep values reset
setYaw = yaw;
magalign_multiplier = 0;
}
if (!armed_gt) { //check if gametrak will be armed
if (gt_length>gt_length_set && gt_length<gt_length_set+10) { //is in trackable length
armed_gt=true;
}
}else if (armed_gt && !armed){ //gametrak control active and not remote active
//Gametrak Control Code
motorenabled=true;
if (gt_length<=GT_LENGTH_MIN){ //let go
armed_gt=false;
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){
_gt_length_diff=0; //threshold
}
out_speed = constrain((int16_t)(_gt_length_diff*gt_speed_p),-GT_SPEED_LIMIT,GT_SPEED_LIMIT);
out_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 (!armed && !armed_gt){ //all disarmed
out_steer = 0;
out_speed = 0;
}
if (millis() - last_send > SENDPERIOD) {
//calculate checksum