interfisch
/
bobbycar
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity
bobbycar/controller/controller.ino

853 lines
31 KiB
C++
Raw Blame History

//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());
}
}
}
Reference in New Issue View Git Blame Copy Permalink