//https://github.com/rogerclarkmelbourne/Arduino_STM32 in arduino/hardware //Board: Generic STM32F103C series //Upload method: serial //20k RAM 64k Flash //may need 3v3 from usb ttl converter (hold down flash button while connecting). Holding down the power button is not needed in this case. //Sometimes reconnecting the usb ttl converter to the pc helps just before pressing the upload button /* TODO: * Do not immediately drive backwards. */ // RX(green) is A10 , TX (blue) ist A9 (3v3 level) //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 proPIN_GAMETRAK_VERTICALgram on powerup // ########################## DEFINES ########################## #define SERIAL_CONTROL_BAUD 38400 // [-] Baud rate for HoverSerial (used to communicate with the hoverboard) #define SERIAL_BAUD 115200 // [-] Baud rate for built-in Serial (used for the Serial Monitor) #define START_FRAME 0xAAAA // [-] Start frme definition for reliable serial communication //#define DEBUG_RX // [-] Debug received data. Prints all bytes to serial (comment-out to disable) //#define MAXADCVALUE 4095 #define ADC_CALIB_THROTTLE_LOWEST 1900 //a bit above adc value if throttle it not touched #define ADC_CALIB_THROTTLE_MIN 2000 //minimum adc value that should correspond to 0 speed #define ADC_CALIB_THROTTLE_MAX 3110 //maximum adc value that should correspond to full speed #define ADC_CALIB_BRAKE_MIN 800 //minimum adc value that should correspond to 0 speed #define ADC_CALIB_BRAKE_MAX 2400 //maximum adc value that should correspond to full speed #define PIN_STARTLED PA0 //Red LED inside Engine Start Button. Powered with 5V via transistor uint8_t startled=0; #define PIN_STARTBUTTON PB8 //"Enginge Start" Button. connected To NC (=LOW). HIGH when pressed #define STARTBUTTON_DOWN digitalRead(PIN_STARTBUTTON) #define SENDPERIOD 50 //ms. delay for sending speed and steer data to motor controller via serial #define PIN_THROTTLE PA4 #define PIN_BRAKE PA5 #define PIN_ENABLE PB9 #define PIN_MODESWITCH PB5 // LOW if pressed in ("down") #define MODESWITCH_DOWN !digitalRead(PIN_MODESWITCH) #define PIN_MODELED_GREEN PA12 #define PIN_MODELED_RED PA11 uint8_t modeled_green=0; uint8_t modeled_red=0; long last_ledupdate=0; #define PIN_RELAISFRONT PB15 //connected to relais which presses the powerbutton of the hoverboard for the front wheels #define PIN_RELAISREAR PB14 //connected to relais which presses the powerbutton of the hoverboard for the rear wheels #define DEBOUNCETIME 20 //time to not check for inputs after key press #define BUTTONTIMEHOLD 750 //time for button hold long millis_lastinput=0; //for button debounce long timebuttonpressed_start; boolean button_start=false; boolean button_hold_start=false; #define TIME_AUTOPOWEROFF 600000 //600000 = 10 minutes long loopmillis=0; //only use one millis reading each loop long last_looptime=0; //for looptiming #define LOOPTIME 10 //how often the loop(s) should run long millis_lastchange=0; //for poweroff after some time with no movement #define MAXBRAKERATE 7 //maximum rate for braking (loop timing) String errormessage=""; //store some error message to print //Mode change variables uint8_t state_modechange=0; long state_modechange_time=0; long millis_lastadc=0; #define ADC_READTIME 10 //time interval to read adc (for filtering) #define ADC_THROTTLE_FILTER 0.4 //low value = slower change #define ADC_BRAKE_FILTER 0.4 //low value = slower change int adc_throttle_raw=0; //raw throttle value from adc float adc_throttle=0; //filtered value int adc_brake_raw=0; //raw throttle value from adc float adc_brake=0; //filtered value int16_t out_speedFL=0; int16_t out_speedFR=0; int16_t out_speedRL=0; int16_t out_speedRR=0; long last_send = 0; boolean board1Enabled=false; boolean board2Enabled=false; // Global variables for serial communication //Serial1 (Rear) uint8_t idx1 = 0; // Index for new data pointer uint16_t bufStartFrame1; // Buffer Start Frame byte *p1; // Pointer declaration for the new received data byte incomingByte1; byte incomingBytePrev1; long lastValidDataSerial1_time; long board1lastPoweron=0; //mainly for failcheck long board1lastPoweroff=0; //Same for Serial2 (Front) uint8_t idx2 = 0; // Index for new data pointer uint16_t bufStartFrame2; // Buffer Start Frame byte *p2; // Pointer declaration for the new received data byte incomingByte2; byte incomingBytePrev2; long lastValidDataSerial2_time; long board2lastPoweron=0; //mainly for failcheck long board2lastPoweroff=0; typedef struct{ uint16_t start; int16_t speedLeft; int16_t speedRight; uint16_t checksum; } SerialCommand; SerialCommand Command1; SerialCommand Command2; typedef struct{ uint16_t start; int16_t cmd1; int16_t cmd2; int16_t speedL; int16_t speedR; int16_t speedL_meas; int16_t speedR_meas; int16_t batVoltage; int16_t boardTemp; int16_t curL_DC; int16_t curR_DC; uint16_t checksum; } SerialFeedback; SerialFeedback Feedback1; SerialFeedback NewFeedback1; SerialFeedback Feedback2; SerialFeedback NewFeedback2; #define A2BIT_CONV 50 //divide curL_DC value by A2BIT_CONV to get current in amperes. Take this value from hoverboard firmware config.h enum mode{booting, idle, on, error, off}; /* * idle: controller is on, hoverboards are off * on: hoverbaords are on and happy * error: some error occured, stop everything and show errors * off: shutdown triggered. will power down latch soon */ mode currentmode; //current active mode mode requestmode; //change this variable to initiate a mode change mode last_requestmode=off; //for printout mode last_currentmode=off; //for printout // ########################## SETUP ########################## void setup() { Serial.begin(SERIAL_BAUD); //Debug and Program. A9=TX1, A10=RX1 (3v3 level) Serial2.begin(SERIAL_CONTROL_BAUD); //control. A2=TX2, A3=RX2 (Serial1 is Usart 2). Marked with "1" on connector (Rear) Serial3.begin(SERIAL_CONTROL_BAUD); //control. B10=TX3, B11=RX3 (Serial2 is Usart 3). Marked with "II" on connector (Front) // Pin Setup pinMode(PIN_STARTLED, OUTPUT); //MODE=PWM (needs testing, mcu locks up when using writePWM() pinMode(PIN_ENABLE, OUTPUT); digitalWrite(PIN_ENABLE, HIGH); //keep power on pinMode(PIN_STARTBUTTON, INPUT_PULLUP); pinMode(PIN_MODESWITCH, INPUT_PULLUP); pinMode(PIN_MODELED_GREEN, OUTPUT); pinMode(PIN_MODELED_RED, OUTPUT); pinMode(PIN_RELAISFRONT, OUTPUT); pinMode(PIN_RELAISREAR, OUTPUT); pinMode(PIN_THROTTLE, INPUT); pinMode(PIN_BRAKE, INPUT); Serial.println("Initialized Serial"); Serial2.println("Initialized Serial1"); Serial3.println("Initialized Serial2"); currentmode = booting; //start in idle mode requestmode = currentmode; millis_lastchange=millis(); } // ########################## LOOP ########################## void loop() { loopmillis=millis(); //read millis for this cycle ReceiveSerial1(); // Check for new received data ReceiveSerial2(); // Check for new received data handleInputs(); if (button_start) { Serial.println("button_start"); } if (button_hold_start) { Serial.println("button_hold_start"); } handleModeChange(); //mode changes if (last_requestmode!=requestmode) { Serial.print("requestmode="); Serial.println(modeToString(requestmode)); last_requestmode=requestmode; } if (last_currentmode!=currentmode) { Serial.print("currentmode="); Serial.println(modeToString(currentmode)); last_currentmode=currentmode; } ledUpdate(); modeloops(); if (loopmillis - last_send > SENDPERIOD) { last_send=loopmillis; if (currentmode!=off || currentmode!=idle) { //if boards should be powered on SendSerial2(out_speedFL,out_speedFR); //Front SendSerial1(out_speedRL,out_speedRR); //Rear } if (currentmode==on) { /*Serial.print("lastData1="); Serial.print(loopmillis-lastValidDataSerial1_time); Serial.print(", lastData2=");Serial.print(loopmillis-lastValidDataSerial2_time); Serial.print(", speedFL="); Serial.print(out_speedFL); float _current = (Feedback1.curL_DC+Feedback1.curR_DC+Feedback2.curL_DC+Feedback2.curR_DC)/4.0 / 50; Serial.print(", current="); Serial.print(_current);*/ float _current = (Feedback1.curL_DC+Feedback1.curR_DC+Feedback2.curL_DC+Feedback2.curR_DC)/4.0 / 50; Serial.print(Feedback1.curL_DC); Serial.print(", "); //1 is rear Serial.print(Feedback1.curR_DC); Serial.print(", "); Serial.print(Feedback2.curL_DC); Serial.print(", "); //2 is front Serial.print(Feedback2.curR_DC); Serial.print(", mean="); Serial.print(_current); Serial.println(); } } if (currentmode!=error) { //keep last errormessage failChecks(); } } void handleInputs() { //Short press (true when button short pressed, on release) button_start=false; //long press (true when button is held down for BUTTONTIMEHOLD, on time elapsed) button_hold_start=false; if (loopmillis-millis_lastinput>DEBOUNCETIME) //Button debouncing { //Trigger if (timebuttonpressed_start == 0 && STARTBUTTON_DOWN){ //first time pressed down. (low when pressed) timebuttonpressed_start=loopmillis; //set time of button press millis_lastinput=loopmillis; }else if(timebuttonpressed_start != 0 && !STARTBUTTON_DOWN){ //button released (was pressed) if (loopmillis-timebuttonpressed_start < BUTTONTIMEHOLD){ //short press button_start=true; } timebuttonpressed_start=0; //re-enable after short press and release from hold millis_lastinput=loopmillis; }else if(loopmillis-timebuttonpressed_start >= BUTTONTIMEHOLD && timebuttonpressed_start>0){ //held down long enough and not already hold triggered button_hold_start=true; timebuttonpressed_start=-1; //-1 as flag for hold triggered } } if ( button_start || button_hold_start) { millis_lastchange=loopmillis; //for auto poweroff millis_lastinput=loopmillis; //for debouncing } if (loopmillis-millis_lastadc>ADC_READTIME) { adc_throttle_raw = analogRead(PIN_THROTTLE); adc_throttle = adc_throttle*(1-ADC_THROTTLE_FILTER) + adc_throttle_raw*ADC_THROTTLE_FILTER; adc_brake_raw = analogRead(PIN_BRAKE); adc_brake = adc_brake*(1-ADC_BRAKE_FILTER) + adc_brake_raw*ADC_BRAKE_FILTER; if (adc_throttle_raw >= ADC_CALIB_THROTTLE_MIN || adc_brake_raw >= ADC_CALIB_BRAKE_MIN) { //throttle or brake pressed millis_lastchange=loopmillis; } millis_lastadc=loopmillis; } if (loopmillis-millis_lastchange>TIME_AUTOPOWEROFF){ requestmode = off; } } void handleModeChange() { if (currentmode==requestmode) { //## Not currently changing modes ## switch (currentmode) { //mode dependant case booting: //on startup. active while start button is still pressed if (button_start) { //button first release requestmode=idle; //start in idle state state_modechange=0; //reset state for safety }//TODO else if (button_hold_start) { requestmode=on; } break; case idle: if (button_hold_start){ //long press requestmode=on; //long press switches betweeen idle and on state_modechange=0; //start at state 0 } if (button_start) { //short press requestmode=off; state_modechange=0; } break; case on: if (button_hold_start){ //long press requestmode=idle; //long press switches betweeen idle and on state_modechange=0; //start at state 0 } if (button_start) { //short press requestmode=off; state_modechange=0; } break; case error: if (button_start) { //short press requestmode=off; state_modechange=0; } break; case off: break; default: currentmode=error; //something else? -> error } }else{ // ## Change requested ## switch (requestmode) { //mode changes case booting: requestmode=error; currentmode=requestmode; errormessage="Change to booting mode cannot be requested"; break; case idle: case on: case off: //similar for on, idle and off if (currentmode == booting) { //coming from booting mode currentmode=idle; //switch directly without powering boards requestmode=currentmode; //make shure it stay in this mode state_modechange=0; break; } if ( (state_modechange>0 || (requestmode==idle && boardsPowered()) || (requestmode==off && boardsPowered()) || (requestmode==on && !boardsPowered()) )) { //power cylce in progress OR need to power on/off boards //Hoverboard powering switch(state_modechange) { case 0: if (requestmode==on && (adc_throttle > ADC_CALIB_THROTTLE_LOWEST || adc_brake > ADC_CALIB_BRAKE_MIN) ) { //requested to turn on but throttle or brake is pressed state_modechange=0; requestmode=currentmode; //abort modechange //TODO: led show aborted modechange }else{ //everythings fine, turn on/off digitalWrite(PIN_RELAISFRONT,HIGH); //simulate hoverboard power button press //Front board is Serial2 if (requestmode==on) { board2Enabled=true; //assume board is online board2lastPoweron=loopmillis; //save time at which board was powered on // ### Request Idle or Off ### }else if(requestmode==idle || requestmode==off) { board2Enabled=false; //assume board is offline board2lastPoweroff=loopmillis; //save time at which board was powered off } state_modechange++; state_modechange_time=loopmillis; //set to current time Serial.println("PIN_RELAISFRONT,HIGH"); } break; case 1: if (loopmillis - state_modechange_time > 200) { //wait some time digitalWrite(PIN_RELAISFRONT,LOW); //release simulated button state_modechange++; state_modechange_time=loopmillis; //set to current time Serial.println("PIN_RELAISFRONT,LOW"); } break; case 2: if (loopmillis - state_modechange_time > 200) { //wait some time digitalWrite(PIN_RELAISREAR,HIGH); //simulate hoverboard power button press //Rear board is Serial1 if (requestmode==on) { board1Enabled=true; //assume board is online board1lastPoweron=loopmillis; //save time at which board was powered on // ### Request Idle or Off ### }else if(requestmode==idle || requestmode==off) { board1Enabled=false; //assume board is offline board1lastPoweroff=loopmillis; //save time at which board was powered off } state_modechange++; state_modechange_time=loopmillis; //set to current time Serial.println("PIN_RELAISREAR,HIGH"); } break; case 3: if (loopmillis - state_modechange_time > 200) { //wait some time digitalWrite(PIN_RELAISREAR,LOW); //release simulated button state_modechange++; state_modechange_time=loopmillis; //set to current time Serial.println("PIN_RELAISREAR,LOW"); } break; case 4: if (loopmillis - state_modechange_time > 1000) { //wait some time after turning on/off state_modechange++; state_modechange_time=loopmillis; //set to current time Serial.println("Waiting finished"); } break; default: //finished modechange currentmode=requestmode; state_modechange=0; break; } }else{ currentmode=requestmode; state_modechange=0; //for safety //Should not happen Serial.print("Warning: power cycle not needed. board1Enabled="); Serial.print(board1Enabled); Serial.print("board2Enabled="); Serial.println(board2Enabled); } break; case error: currentmode=error; //stay in this mode break; default: currentmode=error; } } } void modeloops() { if (loopmillis - last_looptime >= LOOPTIME) { last_looptime=loopmillis; //loop_test(); //for testing (adc calibration prints). comment out following switch case switch (currentmode) { //mode changes case booting: break; case idle: loop_idle(); break; case on: loop_on(); break; case error: loop_error(); break; case off: loop_off(); break; } } } void loop_idle() { out_speedFL=out_speedFR=out_speedRR=out_speedRL=0; //stop motors } void loop_on() { int _maxspeed=1000; int _maxbrake=400; if (MODESWITCH_DOWN) { _maxspeed=200; _maxbrake=200; } /* uint16_t throttlevalue=constrain( map(adc_throttle, ADC_CALIB_THROTTLE_MIN, ADC_CALIB_THROTTLE_MAX, 0, _maxspeed ) ,0, _maxspeed); int16_t brakevalue=constrain( map(adc_brake, ADC_CALIB_BRAKE_MIN, ADC_CALIB_BRAKE_MAX, 0, _maxbrake ) ,0, _maxbrake); //positive value for braking int16_t speedvalue=throttlevalue*(1- (((float)brakevalue)/_maxbrake)) - (brakevalue*(1- (((float)throttlevalue)/_maxspeed)) ); //brake reduces throttle and adds negative torque */ int16_t speedvalue = (out_speedFL+out_speedFR+out_speedRL+out_speedRR)/4; //generate last speedvalue from individual motor speeds uint16_t throttlevalue=constrain( map(adc_throttle, ADC_CALIB_THROTTLE_MIN, ADC_CALIB_THROTTLE_MAX, 0, _maxspeed ) ,0, _maxspeed); int16_t brakevalue=constrain( map(adc_brake, ADC_CALIB_BRAKE_MIN, ADC_CALIB_BRAKE_MAX, 0, _maxbrake ) ,0, _maxbrake); //positive value for braking int16_t combthrottlevalue=throttlevalue*(1- (((float)brakevalue)/_maxbrake)) - (brakevalue*(1- (((float)throttlevalue)/_maxspeed)) ); //brake reduces throttle and adds negative torque int16_t combthrottlevalue_positive = max((int16_t)0,combthrottlevalue); //only positive #define CURRENTBRAKE_P 5.0 //proportional brake when throttle is lower than current speed. Depends on LOOPTIME #define BRAKE_P 0.02 //speed-=brakevalue*brake_p . depends on LOOPTIME //serial2 is Front. serial1 is Rear float _current = (Feedback1.curL_DC+Feedback1.curR_DC+Feedback2.curL_DC+Feedback2.curR_DC)/4.0 / A2BIT_CONV; if (combthrottlevalue_positive>=speedvalue) { //if throttle higher then apply immediately speedvalue = combthrottlevalue_positive; }else{ //throttle lever is lower than current set speedvalue if (_current > 0) { //is consuming current when it shouldnt speedvalue = max( (int16_t)(speedvalue-_current*CURRENTBRAKE_P) ,combthrottlevalue_positive); //not lower than throttlevalue } } if (combthrottlevalue<0){ //throttle off and brake pressed speedvalue= max((int16_t)(speedvalue + combthrottlevalue*BRAKE_P),(int16_t)0); //not negative = not backwards } speedvalue = throttlevalue; //TEST OVERRIDE out_speedFL=speedvalue; out_speedFR=speedvalue; out_speedRL=speedvalue; out_speedRR=speedvalue; } void loop_error() { out_speedFL=out_speedFR=out_speedRR=out_speedRL=0; //stop motors Serial.print("Error:"); Serial.println(errormessage); } void loop_test() { Serial.print("adc_throttle_raw="); Serial.print(adc_throttle_raw); Serial.print(", adc_brake_raw="); Serial.print(adc_brake_raw); int _maxspeed=1000; int _maxbrake=400; if (MODESWITCH_DOWN) { _maxspeed=200; _maxbrake=200; } int16_t throttlevalue=constrain( map(adc_throttle, ADC_CALIB_THROTTLE_MIN, ADC_CALIB_THROTTLE_MAX, 0, _maxspeed ) ,0, _maxspeed); int16_t brakevalue=constrain( map(adc_brake, ADC_CALIB_BRAKE_MIN, ADC_CALIB_BRAKE_MAX, 0, _maxbrake ) ,0, _maxbrake); //positive value for braking int16_t speedvalue=throttlevalue*(1- (((float)brakevalue)/_maxbrake)) - (brakevalue*(1- (((float)throttlevalue)/_maxspeed)) ); //brake reduces throttle and adds negative torque Serial.print(", throttle="); Serial.print(throttlevalue); Serial.print(", brake="); Serial.print(brakevalue); Serial.print(", speed="); Serial.println(speedvalue); } void loop_off() { //loop enters when boards are sucessfully turned off digitalWrite(PIN_ENABLE, LOW); //cut own power } boolean boardsPowered() { return (board1Enabled && board2Enabled); //true if both boards enabled } void failChecks() { #define FAILCHECK_WAITCHECK_AFTER_POWEROFF_TIME 1000 //time to start failchecking boardpower after board poweroff #define FAILCHECK_RECEIVERECENT_TIME 100 //timeout .should be less than FAILCHECK_WAITCHECK_AFTER_POWEROFF_TIME and greater than send delay from mainboard // ## Check if board is really offline ## if (!board1Enabled) { //board should be offline if (loopmillis-board1lastPoweroff > FAILCHECK_WAITCHECK_AFTER_POWEROFF_TIME){ //wait some time before checking if board did power off if (loopmillis-lastValidDataSerial1_time < FAILCHECK_RECEIVERECENT_TIME) { //new message received recently? errormessage="Board 1 should be offline but feedback received"; Serial.println(errormessage); requestmode=error; } } } if (!board2Enabled) { //board should be offline if (loopmillis-board2lastPoweroff > FAILCHECK_WAITCHECK_AFTER_POWEROFF_TIME){ //wait some time before checking if board did power off if (loopmillis-lastValidDataSerial2_time < FAILCHECK_RECEIVERECENT_TIME) { //new message received recently? errormessage="Board 2 should be offline but feedback received"; Serial.println(errormessage); requestmode=error; } } } #define FAILCHECK_WAITCHECK_AFTER_POWERON_TIME 2000 //time to start failchecking boardpower after startup // ## Check if board is online (when it should send feedback) ## if (board1Enabled) { //board should be online if (loopmillis-board1lastPoweron > FAILCHECK_WAITCHECK_AFTER_POWERON_TIME) { //wait some time before checking if (loopmillis-lastValidDataSerial1_time > FAILCHECK_RECEIVERECENT_TIME) { //speed still high enough but no new messages recently received? errormessage="Board 1 should be online and give feedback but didnt"; Serial.println(errormessage); requestmode=error; } } } if (board2Enabled) { //board should be online if (loopmillis-board2lastPoweron > FAILCHECK_WAITCHECK_AFTER_POWERON_TIME) { //wait some time before checking if (loopmillis-lastValidDataSerial2_time > FAILCHECK_RECEIVERECENT_TIME) { //no new messages recently received? errormessage="Board 2 should be online and give feedback but didnt"; Serial.println(errormessage); requestmode=error; } } } } String modeToString(uint8_t m) { if (m==idle) return "idle"; if (m==off) return "off"; if (m==error) return "error"; if (m==on) return "on"; if (m==booting) return "booting"; } void ledUpdate() { #define LEDUPDATETIME 20 #define FASTERRORBLINKDELAY 100 //period for startled to blink on error if (loopmillis - last_ledupdate >= LEDUPDATETIME) { last_ledupdate=loopmillis; // ## StartLed ## uint8_t _ledbrightness; switch (currentmode) { //modeLed for different currentmodes case booting: startled=255; break; case idle: //Breathing Startled _ledbrightness=uint8_t( (loopmillis/10)%(512) ); startled=_ledbrightness<=255 ? _ledbrightness : (512)-_ledbrightness; //reverse if >255 to go down again break; case on: //Startled on startled=255; break; case error: //Startled blink startled=(loopmillis/FASTERRORBLINKDELAY)%2==0 ? 0 : 255; // Blink led break; case off: //Startled off startled=0; break; } // ## ModeLed ## if (currentmode!=requestmode) { //ongoing modechange modeled_green=0; modeled_red=0; //ModeLed=Off }else{ switch (currentmode) { //modeLed for different currentmodes case booting: modeled_green=255; modeled_red=0; //ModeLed=Green break; case idle: modeled_green=255; modeled_red=255; //ModeLed=Yellow break; case on: modeled_green=255; modeled_red=0; //ModeLed=Green break; case error: modeled_green=0; modeled_red=(loopmillis/FASTERRORBLINKDELAY)%2==0 ? 0 : 255; // Blink led , ModeLed=Red break; case off: modeled_green=255; modeled_red=255; //ModeLed=Yellow break; } } /* pwmWrite(PIN_MODELED_GREEN, map(modeled_green, 0, 255, 0, 65535)); pwmWrite(PIN_MODELED_RED, map(modeled_red, 0, 255, 0, 65535)); pwmWrite(PIN_STARTLED, map(startled, 0, 255, 0, 65535)); */ digitalWrite(PIN_MODELED_GREEN, modeled_green<127? true:false); //red and green inverted (common anode) digitalWrite(PIN_MODELED_RED, modeled_red<127? true:false); //red and green inverted (common anode) digitalWrite(PIN_STARTLED, startled>127? true:false); } } /* // Old loop void loopold() { //selfTest(); //start selftest, does not return ReceiveSerial1(); // Check for new received data if (millis()>2000 && STARTBUTTON_DOWN) { poweronBoards(); } if (millis() - last_send > SENDPERIOD) { //Serial.print("powerbutton="); Serial.print(STARTBUTTON_DOWN); Serial.print(" modeswitch down="); Serial.println(MODESWITCH_DOWN); int _read=analogRead(PIN_THROTTLE); int16_t speedvalue=constrain( map(_read, ADC_CALIB_THROTTLE_MIN, ADC_CALIB_THROTTLE_MAX, 0, 1000 ) ,0, 1000); if (MODESWITCH_DOWN) { SendSerial1(speedvalue,0); SendSerial2(speedvalue,0); Serial.print("L_"); }else{ SendSerial1(0,speedvalue); SendSerial2(0,speedvalue); Serial.print("R_"); } Serial.print("millis="); Serial.print(millis()); Serial.print(", adcthrottle="); Serial.print(_read); Serial.print(", 1.L="); Serial.print(Command1.speedLeft); Serial.print(", 1.R="); Serial.print(Command1.speedRight); Serial.print(", 2.L="); Serial.print(Command2.speedLeft); Serial.print(", 2.R="); Serial.println(Command2.speedRight); last_send = millis(); digitalWrite(PIN_STARTLED, !digitalRead(PIN_STARTLED)); if (testcounter%3==0) { digitalWrite(PIN_MODELED_GREEN, !digitalRead(PIN_MODELED_GREEN)); } if (testcounter%5==0) { digitalWrite(PIN_MODELED_RED, !digitalRead(PIN_MODELED_RED)); } testcounter++; //Print Motor values Serial.print("cmd1"); Serial.print(", "); Serial.print("cmd2"); Serial.print(","); Serial.print("speedR"); Serial.print(","); Serial.print("speedL"); Serial.print(", "); Serial.print("speedR_meas"); Serial.print(","); Serial.print("speedL_meas"); Serial.print(", "); Serial.print("batVoltage"); Serial.print(", "); Serial.println("boardTemp"); Serial.println(); Serial.print("1: "); Serial.print(Feedback1.cmd1); Serial.print(", "); Serial.print(Feedback1.cmd2); Serial.print(","); Serial.print(Feedback1.speedR); Serial.print(","); Serial.print(Feedback1.speedL); Serial.print(", "); Serial.print(Feedback1.speedR_meas); Serial.print(","); Serial.print(Feedback1.speedL_meas); Serial.print(", "); Serial.print(Feedback1.batVoltage); Serial.print(", "); Serial.println(Feedback1.boardTemp); Serial.println(); Serial.print("2: "); Serial.print(Feedback2.cmd1); Serial.print(", "); Serial.print(Feedback2.cmd2); Serial.print(","); Serial.print(Feedback2.speedR); Serial.print(","); Serial.print(Feedback2.speedL); Serial.print(", "); Serial.print(Feedback2.speedR_meas); Serial.print(","); Serial.print(Feedback2.speedL_meas); Serial.print(", "); Serial.print(Feedback2.batVoltage); Serial.print(", "); Serial.println(Feedback2.boardTemp); } if (millis()>30000 && STARTBUTTON_DOWN) { poweroff(); } } // ########################## END ########################## void poweroff() { //TODO: trigger Relais for Board 1 // Wait for board to shut down //TODO: trigger Relais for Board 2 // Wait for board to shut down //Timeout error handling digitalWrite(PIN_ENABLE, LOW); //poweroff own latch delay(1000); Serial.println("Still powered"); //still powered on: set error status "power latch error" } void poweronBoards() { digitalWrite(PIN_RELAISFRONT,HIGH); delay(200);digitalWrite(PIN_RELAISFRONT,LOW); delay(50); digitalWrite(PIN_RELAISREAR,HIGH); delay(200);digitalWrite(PIN_RELAISREAR,LOW); } */ void selfTest() { digitalWrite(PIN_ENABLE,HIGH); //make shure latch is on Serial.println("Entering selftest"); #define TESTDELAY 1000 //delay between test #define TESTTIME 500 //time to keep tested pin on delay(TESTDELAY); Serial.println("PIN_STARTLED"); digitalWrite(PIN_STARTLED,HIGH); delay(TESTTIME); digitalWrite(PIN_STARTLED,LOW); delay(TESTDELAY); Serial.println("PIN_MODELED_GREEN"); digitalWrite(PIN_MODELED_GREEN,LOW); delay(TESTTIME); digitalWrite(PIN_MODELED_GREEN,HIGH); delay(TESTDELAY); Serial.println("PIN_MODELED_RED"); digitalWrite(PIN_MODELED_RED,LOW); delay(TESTTIME); digitalWrite(PIN_MODELED_RED,HIGH); delay(TESTDELAY); Serial.println("PIN_RELAISFRONT"); digitalWrite(PIN_RELAISFRONT,HIGH); delay(TESTTIME); digitalWrite(PIN_RELAISFRONT,LOW); delay(TESTDELAY); Serial.println("PIN_RELAISREAR"); digitalWrite(PIN_RELAISREAR,HIGH); delay(TESTTIME); digitalWrite(PIN_RELAISREAR,LOW); delay(TESTDELAY); Serial.println("ALL ON"); digitalWrite(PIN_STARTLED,HIGH); digitalWrite(PIN_MODELED_GREEN,LOW); digitalWrite(PIN_MODELED_RED,LOW); digitalWrite(PIN_RELAISFRONT,HIGH); digitalWrite(PIN_RELAISREAR,HIGH); delay(TESTTIME*5); digitalWrite(PIN_STARTLED,LOW); digitalWrite(PIN_MODELED_GREEN,HIGH); digitalWrite(PIN_MODELED_RED,HIGH); digitalWrite(PIN_RELAISFRONT,LOW); digitalWrite(PIN_RELAISREAR,LOW); delay(TESTDELAY); Serial.println("Powers off latch at millis>=60000"); Serial.println("Inputs:"); while(true) { //Keep printing input values forever delay(100); Serial.print("millis="); Serial.print(millis()); Serial.print(", throttle ADC="); Serial.println(analogRead(PIN_THROTTLE)); Serial.print("powerbutton down="); Serial.print(STARTBUTTON_DOWN); Serial.print(" modeswitch down="); Serial.println(MODESWITCH_DOWN); while (millis()>=60000) { digitalWrite(PIN_ENABLE, LOW); //poweroff own latch Serial.println(millis()); } } }