diff --git a/controller.ino b/controller.ino index 9d71992..83bc9d2 100644 --- a/controller.ino +++ b/controller.ino @@ -7,7 +7,10 @@ //to flash set boot0 (the one further away from reset button) to 1 and press reset, flash, program executes immediately //set boot0 back to 0 to run program on powerup +//PA2 may be defective on my bluepill + //#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; @@ -15,24 +18,49 @@ uint8_t imu_no_change_counter = 0; #define PIN_LED PC13 #define PIN_VBAT PA0 //battery voltage after voltage divider -#define VBAT_DIV_FACTOR 0.010700 //how much voltage (V) equals one adc unit. measured at 40V and averaged +//#define VBAT_DIV_FACTOR 0.010700 //how much voltage (V) equals one adc unit. measured at 40V and averaged +#define VBAT_DIV_FACTOR 0.01399535423925667828 //how much voltage (V) equals one adc unit. 3444=48.2V #define PIN_CURRENT PA1 //output of hall sensor for current measurement -#define CURRENT_OFFSET 2034 //adc reading at 0A, with CJMCU-758 typically at Vcc/2 -#define CURRENT_FACTOR 0.4320376 //how much current (A) equals one adc unit -double vbat=0; //battery voltage -double ibat=0; //battery current -long last_uiupdated=0; -#define UI_UPDATEPERIOD 10 //in ms +#define CURRENT_OFFSET 2048 //adc reading at 0A, with CJMCU-758 typically at Vcc/2. measured with actual voltage supply in hoverbrett +#define CURRENT_FACTOR 0.38461538461538461538 //how much current (A) equals one adc unit. 2045-2032=13 at 5A +float vbat=0; //battery voltage +float ibat=0; //battery current +long last_adcupdated=0; +#define ADC_UPDATEPERIOD 10 //in ms -#define SENDPERIOD 20 //ms +#define SENDPERIOD 20 //ms. delay for sending speed and steer data to motor controller via serial + +//Status information sending +#define PARAMETERSENDPERIOD 200 //delay for sending stat data via nrf24 +long last_parametersend=0; #define CONTROLUPDATEPERIOD 10 long last_controlupdate = 0; -#define IMUUPDATEPERIOD 10 //ms -long last_imuupdated = 0; -#define MAX_YAWCHANGE 90 //in degrees, if exceeded in one update intervall error will be triggered +#define PIN_GAMETRAK_LENGTH PA1 //yellow (connector) / orange (gametrak module wires): length +#define PIN_GAMETRAK_VERTICAL PA3 //orange / red: vertical +#define PIN_GAMETRAK_HORIZONTAL PA4 //blue / yellow: horizontal + + +#define GT_LENGTH_OFFSET 4090 //adc offset value (rolled up value) +#define GT_LENGTH_MIN 220 //length in mm at which adc values start to change +#define GT_LENGTH_SCALE -0.73 //(adcvalue-offset)*scale = length[mm] (+length_min) +//2720 at 1000mm+220mm -> 1370 for 1000mm -> +#define GT_LENGTH_MAXLENGTH 2500 //maximum length in [mm]. maximum string length is around 2m80 +uint16_t gt_length=0; //0=rolled up, 1unit = 1mm + +#define GT_VERTICAL_CENTER 2048 //adc value for center position +#define GT_VERTICAL_RANGE 2047 //adc value difference from center to maximum (30 deg) +int8_t gt_vertical=0; //0=center. joystick can rotate +-30 degrees. -127 = -30 deg +//left = -30 deg, right= 30deg + +#define GT_HORIZONTAL_CENTER 2048 //adc value for center position +#define GT_HORIZONTAL_RANGE 2047 //adc value difference from center to maximum (30 deg) +int8_t gt_horizontal=0; //0=center + + + #include @@ -41,6 +69,10 @@ long last_imuupdated = 0; //https://github.com/mechasolution/Mecha_QMC5883L //because QMC5883 on GY85 instead of HMC5883, source: https://circuitdigest.com/microcontroller-projects/digital-compass-with-arduino-and-hmc5883l-magnetometer //in qmc5883L library read values changed from uint16_t to int16_t +#define IMUUPDATEPERIOD 10 //ms +long last_imuupdated = 0; +#define MAX_YAWCHANGE 90 //in degrees, if exceeded in one update intervall error will be triggered + IMUGY85 imu; double ax, ay, az, gx, gy, gz, roll, pitch, yaw, mx, my, mz, ma; double old_ax, old_ay, old_az, old_gx, old_gy, old_gz, old_roll, old_pitch, old_yaw, old_mx, old_my, old_mz, old_ma; @@ -48,7 +80,7 @@ double old_ax, old_ay, old_az, old_gx, old_gy, old_gz, old_roll, old_pitch, old_ double setYaw = 0; float magalign_multiplier = 0; //how much the magnetometer should influence steering, 0=none, 1=stay aligned - +// Lenovo Trackpoint pinout //from left to right. pins at bottom. chips on top //1 GND (black) //2 Data @@ -86,6 +118,9 @@ long last_nrfreceive = 0; //last time values were received and checksum ok long nrf_delay = 0; #define MAX_NRFDELAY 100 //ms. maximum time delay at which vehicle will disarm +boolean radiosendOk=false; + + //command variables boolean motorenabled = false; //set by nrfdata.commands @@ -96,6 +131,8 @@ 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; uint8_t out_checksum = 0; //0= disable motors, 255=reserved, 1<=checksum<255 #define NRFDATA_CENTER 127 @@ -106,9 +143,10 @@ boolean lastpacketOK = false; void setup() { - Serial.begin(115200); //Debug and Program. A9=TX1, A10=RX1 (3v3 level) + Serial.begin(57600); //Debug and Program. A9=TX1, A10=RX1 (3v3 level) Serial2.begin(19200); //control. B10=TX3, B11=RX3 (Serial2 is Usart 3) + //Serial1 max be dead on my board? pinMode(PIN_LED, OUTPUT); @@ -118,12 +156,18 @@ void setup() { pinMode(PIN_VBAT,INPUT_ANALOG); pinMode(PIN_CURRENT,INPUT_ANALOG); + pinMode(PIN_GAMETRAK_LENGTH,INPUT_ANALOG); + pinMode(PIN_GAMETRAK_VERTICAL,INPUT_ANALOG); + pinMode(PIN_GAMETRAK_HORIZONTAL,INPUT_ANALOG); + #ifdef DEBUG Serial.println("Initializing nrf24"); + #endif radio.begin(); radio.setDataRate( RF24_250KBPS ); //set to slow data rate. default was 1MBPS + //radio.setDataRate( RF24_1MBPS ); radio.setChannel(NRF24CHANNEL); //0 to 124 (inclusive) @@ -135,10 +179,14 @@ void setup() { radio.startListening(); + #ifdef DEBUG Serial.println("Initializing IMU"); + #endif imu.init(); - Serial.println("Initialized"); + #ifdef DEBUG + Serial.println("Initialized"); + #endif } @@ -150,15 +198,36 @@ void loop() { last_imuupdated = millis(); } - if (millis() - last_uiupdated > UI_UPDATEPERIOD) { //update current and voltage + if (millis() - last_adcupdated > ADC_UPDATEPERIOD) { //update analog readings vbat=analogRead(PIN_VBAT)*VBAT_DIV_FACTOR; ibat=(analogRead(PIN_CURRENT)-CURRENT_OFFSET)*CURRENT_FACTOR; - last_uiupdated = millis(); + + + gt_length = constrain((analogRead(PIN_GAMETRAK_LENGTH)-GT_LENGTH_OFFSET)*GT_LENGTH_SCALE +GT_LENGTH_MIN, 0,GT_LENGTH_MAXLENGTH); + if (gt_length<=GT_LENGTH_MIN){ + gt_length=0; //if below minimum measurable length set to 0mm + } + gt_vertical = constrain(map(analogRead(PIN_GAMETRAK_VERTICAL)-GT_VERTICAL_CENTER, +GT_VERTICAL_RANGE,-GT_VERTICAL_RANGE,-127,127),-127,127); //left negative + gt_horizontal = constrain(map(analogRead(PIN_GAMETRAK_HORIZONTAL)-GT_HORIZONTAL_CENTER, +GT_HORIZONTAL_RANGE,-GT_HORIZONTAL_RANGE,-127,127),-127,127); //down negative + + last_adcupdated = millis(); + /* - Serial.print("vbat="); - Serial.print(vbat); - Serial.print(", ibat="); - Serial.println(ibat);*/ + Serial.print("gt_length="); + Serial.print(gt_length); + Serial.print(", gt_vertical="); + Serial.print(gt_vertical); + Serial.print(", gt_horizontal="); + Serial.println(gt_horizontal);*/ + + /* + Serial.print("PIN_GAMETRAK_LENGTH="); + Serial.print(analogRead(PIN_GAMETRAK_LENGTH)); + Serial.print(", PIN_GAMETRAK_VERTICAL="); + Serial.print(analogRead(PIN_GAMETRAK_VERTICAL)); + Serial.print(", PIN_GAMETRAK_HORIZONTAL="); + Serial.println(analogRead(PIN_GAMETRAK_HORIZONTAL)); + */ } //NRF24 @@ -180,7 +249,6 @@ void loop() { - uint8_t calcchecksum = (uint8_t)((lastnrfdata.steer + 3) * (lastnrfdata.speed + 13)); if (lastnrfdata.checksum == calcchecksum) { //checksum ok? lastpacketOK = true; @@ -230,6 +298,7 @@ void loop() { } } + if (error > 0) { //disarm if error occured armed = false; } @@ -282,50 +351,93 @@ void loop() { #ifdef DEBUG if (!lastpacketOK) Serial.println("Armed but packet not ok"); - } + } #endif -} else { //disarmed - out_steer = 0; - out_speed = 0; - setYaw = yaw; - magalign_multiplier = 0; -} - - - - -if (millis() - last_send > SENDPERIOD) { - //calculate checksum - out_checksum = ((uint8_t) ((uint8_t)out_steer) * ((uint8_t)out_speed)); //simple checksum - if (out_checksum == 0 || out_checksum == 255) { - out_checksum = 1; //cannot be 0 or 255 (special purpose) + } else { //disarmed + out_steer = 0; + out_speed = 0; + setYaw = yaw; + magalign_multiplier = 0; } - if (!motorenabled) { //disable motors? - out_checksum = 0; //checksum=0 disables motors + + + + if (millis() - last_send > SENDPERIOD) { + //calculate checksum + out_checksum = ((uint8_t) ((uint8_t)out_steer) * ((uint8_t)out_speed)); //simple checksum + if (out_checksum == 0 || out_checksum == 255) { + out_checksum = 1; //cannot be 0 or 255 (special purpose) + } + + if (!motorenabled) { //disable motors? + 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_checksum, sizeof(out_checksum)); + lastsend_out_steer = out_steer; //remember last transmittet values (for stat sending) + lastsend_out_speed = out_speed; + last_send = millis(); + + + #ifdef DEBUG + Serial.print(" steer="); + Serial.print(out_steer); + Serial.print(" speed="); + Serial.print(out_speed); + Serial.print(" checksum="); + Serial.print(out_checksum); + + Serial.println(); + #endif } - Serial2.write((uint8_t *) &out_steer, sizeof(out_steer)); - Serial2.write((uint8_t *) &out_speed, sizeof(out_speed)); - Serial2.write((uint8_t *) &out_checksum, sizeof(out_checksum)); - last_send = millis(); + + // + #ifdef PARAMETEROUTPUT + if ( millis() - last_parametersend > PARAMETERSENDPERIOD) { + //Serial.write((uint8_t *) &counter, sizeof(counter));//uint8_t, 1 byte + //Serial.write((uint8_t *) &value1, sizeof(value1)); //uint16_t, 2 bytes + //Serial.write((uint8_t *) &value2, sizeof(value2)); //int16_t, 2 bytes + //Serial.write((uint8_t *) &floatvalue, sizeof(floatvalue)); //float, 4 bytes - -#ifdef DEBUG - Serial.print(" steer="); - Serial.print(out_steer); - Serial.print(" speed="); - Serial.print(out_speed); - Serial.print(" checksum="); - Serial.print(out_checksum); - - Serial.println(); -#endif -} + /* + 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 *) &vbat, sizeof(vbat)); //float, 4 bytes + Serial.write((uint8_t *) &ibat, sizeof(ibat)); //float, 4 bytes + float yaw_float=yaw; + Serial.write((uint8_t *) &yaw_float, sizeof(yaw_float)); //float, 4 bytes + */ + + + + last_parametersend = millis(); + } + #endif + } +/* +void sendRF(nrfstatdata senddata){ + #ifdef DEBUG + Serial.println("Transmitting..."); + #endif + + radio.stopListening(); //stop listening to be able to transmit + radiosendOk = radio.write( &senddata, sizeof(nrfstatdata) ); + if (!radiosendOk){ + #ifdef DEBUG + Serial.println("send failed"); + #endif + } + radio.startListening(); //start listening again +} +*/ void updateIMU() { @@ -333,7 +445,9 @@ void updateIMU() imu_no_change_counter++; if (imu_no_change_counter > 10) { error = IMU_NO_CHANGE; + #ifdef DEBUG Serial.println("Error: IMU_NO_CHANGE"); + #endif } } else { imu_no_change_counter = 0;