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

change steer, speed to speedl and speedr

This commit is contained in:
interfisch 2019-06-14 01:01:18 +02:00
parent 216db6080d
commit 090ec067c1
1 changed files with 39 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

64
controller/controller.ino
View File

@ -9,8 +9,8 @@
//PA2 may be defective on my bluepill
#define DEBUG
//#define PARAMETEROUTPUT
//#define DEBUG
#define PARAMETEROUTPUT
uint8_t error = 0;
#define IMU_NO_CHANGE 2 //IMU values did not change for too long
uint8_t imu_no_change_counter = 0;
@ -133,10 +133,12 @@ boolean motorenabled = false; //set by nrfdata.commands
long last_send = 0;
int16_t out_steer = 0; //between -1000 and 1000
int16_t out_speed = 0;
int16_t lastsend_out_steer = 0; //last value transmitted to motor controller
int16_t lastsend_out_speed = 0;
int16_t out_speedl = 0; //between -1000 and 1000
int16_t out_speedr = 0;
int16_t lastsend_out_speedl = 0; //last value transmitted to motor controller
int16_t lastsend_out_speedr = 0;
int16_t set_speed = 0;
int16_t set_steer = 0;
uint8_t out_checksum = 0; //0= disable motors, 255=reserved, 1<=checksum<255
#define NRFDATA_CENTER 127
@ -325,8 +327,8 @@ void loop() {
//out_speed=(int16_t)( (lastnrfdata.y-TRACKPOINT_CENTER)*1000/TRACKPOINT_MAX );
//out_steer=(int16_t)( -(lastnrfdata.x-TRACKPOINT_CENTER)*1000/TRACKPOINT_MAX );
out_speed = (int16_t)( ((int16_t)(lastnrfdata.speed) - NRFDATA_CENTER) * 1000 / 127 ); //-1000 to 1000
out_steer = (int16_t)( ((int16_t)(lastnrfdata.steer) - NRFDATA_CENTER) * 1000 / 127 );
set_speed = (int16_t)( ((int16_t)(lastnrfdata.speed) - NRFDATA_CENTER) * 1000 / 127 ); //-1000 to 1000
set_steer = (int16_t)( ((int16_t)(lastnrfdata.steer) - NRFDATA_CENTER) * 1000 / 127 );
//align to compass
double yawdiff = (setYaw - 180) - (yaw - 180); //following angle difference works only for angles [-180,180]. yaw here is [0,360]
@ -339,7 +341,7 @@ void loop() {
yawdiff = yawdiff * yawdiff; //square
yawdiff = constrain(yawdiff * 1 , 0, 800);
yawdiff *= yawdiffsign; //redo sign
int16_t out_steer_mag = (int16_t)( yawdiff );
int16_t set_steer_mag = (int16_t)( yawdiff );
float new_magalign_multiplier = map( abs((int16_t)(lastnrfdata.steer) - NRFDATA_CENTER), 2, 10, 1.0, 0.0); //0=normal steering, 1=only mag steering
new_magalign_multiplier = 0; //Force mag off
@ -348,10 +350,16 @@ void loop() {
magalign_multiplier = min(new_magalign_multiplier, min(1.0, magalign_multiplier + 0.01)); //go down fast, slowly increase
magalign_multiplier = constrain(magalign_multiplier, 0.0, 1.0); //safety constrain again
out_steer = out_steer * (1 - magalign_multiplier) + out_steer_mag * magalign_multiplier;
set_steer = set_steer * (1 - magalign_multiplier) + set_steer_mag * magalign_multiplier;
setYaw = setYaw * magalign_multiplier + yaw * (1 - magalign_multiplier); //if magalign_multiplier 0, setYaw equals current yaw
//calculate speed l and r from speed and steer
#define SPEED_COEFFICIENT_NRF 1 // higher value == stronger
#define STEER_COEFFICIENT_NRF 0.5 // higher value == stronger
out_speedl = constrain(set_speed * SPEED_COEFFICIENT_NRF + set_steer * STEER_COEFFICIENT_NRF, -1000, 1000);
out_speedr = constrain(set_speed * SPEED_COEFFICIENT_NRF - set_steer * STEER_COEFFICIENT_NRF, -1000, 1000);
/*
Serial.print("Out steer=");
Serial.println(out_steer);*/
@ -389,8 +397,14 @@ void loop() {
_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
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
//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
out_speedl = constrain(set_speed * SPEED_COEFFICIENT_GT + set_steer * STEER_COEFFICIENT_GT, -1000, 1000);
out_speedr = constrain(set_speed * SPEED_COEFFICIENT_GT - set_steer * STEER_COEFFICIENT_GT, -1000, 1000);
}
@ -399,15 +413,15 @@ void loop() {
}
if (controlmode == MODE_DISARMED){ //all disarmed
out_steer = 0;
out_speed = 0;
out_speedl = 0;
out_speedr = 0;
}
if (millis() - last_send > SENDPERIOD) {
//calculate checksum
out_checksum = ((uint8_t) ((uint8_t)out_steer) * ((uint8_t)out_speed)); //simple checksum
out_checksum = ((uint8_t) ((uint8_t)out_speedl) * ((uint8_t)out_speedr)); //simple checksum
if (out_checksum == 0 || out_checksum == 255) {
out_checksum = 1; //cannot be 0 or 255 (special purpose)
}
@ -416,19 +430,19 @@ void loop() {
out_checksum = 0; //checksum=0 disables motors
}
Serial2.write((uint8_t *) &out_steer, sizeof(out_steer));
Serial2.write((uint8_t *) &out_speed, sizeof(out_speed));
Serial2.write((uint8_t *) &out_speedl, sizeof(out_speedl));
Serial2.write((uint8_t *) &out_speedr, sizeof(out_speedr));
Serial2.write((uint8_t *) &out_checksum, sizeof(out_checksum));
lastsend_out_steer = out_steer; //remember last transmittet values (for stat sending)
lastsend_out_speed = out_speed;
lastsend_out_speedl = out_speedl; //remember last transmittet values (for stat sending)
lastsend_out_speedr = out_speedr;
last_send = millis();
#ifdef DEBUG
Serial.print(" steer=");
Serial.print(out_steer);
Serial.print(" speed=");
Serial.print(out_speed);
Serial.print(" out_speedl=");
Serial.print(out_speedl);
Serial.print(" out_speedr=");
Serial.print(out_speedr);
Serial.print(" checksum=");
Serial.print(out_checksum);
@ -453,8 +467,8 @@ void loop() {
booleanvalues |= motorenabled<<0; //bit 0
booleanvalues |= (controlmode&0b00000011)<<1; //bit 1 and 2 (2bit number for controlmodes (3)
Serial.write((uint8_t *) &out_steer, sizeof(out_steer)); //int16_t, 2 bytes
Serial.write((uint8_t *) &out_speed, sizeof(out_speed)); //int16_t, 2 bytes
Serial.write((uint8_t *) &out_speedl, sizeof(out_speedl)); //int16_t, 2 bytes
Serial.write((uint8_t *) &out_speedr, sizeof(out_speedr)); //int16_t, 2 bytes
Serial.write((uint8_t *) &booleanvalues, sizeof(booleanvalues)); //uint8_t, 1 byte //booleanvalues
Serial.write((uint8_t *) &vbat, sizeof(vbat)); //float, 4 bytes
//Serial.write((uint8_t *) &ibat, sizeof(ibat)); //float, 4 bytes