From 090ec067c1c70c7dd94172ce2a4c60fffc359075 Mon Sep 17 00:00:00 2001 From: Fisch Date: Fri, 14 Jun 2019 01:01:18 +0200 Subject: [PATCH] change steer, speed to speedl and speedr --- controller/controller.ino | 64 ++++++++++++++++++++++++--------------- 1 file changed, 39 insertions(+), 25 deletions(-) diff --git a/controller/controller.ino b/controller/controller.ino index c002a4f..2907c47 100644 --- a/controller/controller.ino +++ b/controller/controller.ino @@ -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