add led function
This commit is contained in:
parent
fd3a474f18
commit
fea152adc8
|
@ -76,6 +76,10 @@ float filtered_currentAll=0;
|
|||
int16_t cmd_send=0;
|
||||
int16_t last_cmd_send=0;
|
||||
|
||||
uint8_t speedmode=0;
|
||||
#define SPEEDMODE_SLOW 1
|
||||
#define SPEEDMODE_NORMAL 0
|
||||
|
||||
|
||||
// Global variables for serial communication
|
||||
typedef struct{
|
||||
|
@ -149,6 +153,7 @@ void failChecks();
|
|||
void sendCMD();
|
||||
void checkLog();
|
||||
void updateMotorparams( MotorParameter &mp, SerialFeedback &fb);
|
||||
void leds();
|
||||
|
||||
void SendSerial(SerialCommand &scom, int16_t uSpeedLeft, int16_t uSpeedRight, HardwareSerial &SerialRef)
|
||||
{
|
||||
|
@ -243,7 +248,7 @@ void setup()
|
|||
pinMode(PIN_START, INPUT_PULLUP);
|
||||
|
||||
pinMode(PIN_LED_START, OUTPUT); //Active High
|
||||
digitalWrite(PIN_LED_START,HIGH);
|
||||
|
||||
pinMode(PIN_MODE_LEDG, OUTPUT); //Active Low
|
||||
digitalWrite(PIN_MODE_LEDG,LOW);
|
||||
pinMode(PIN_MODE_LEDR, OUTPUT); //Active Low
|
||||
|
@ -287,6 +292,8 @@ void loop() {
|
|||
//If needed write log to serial port
|
||||
checkLog();
|
||||
|
||||
leds();
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
@ -378,14 +385,19 @@ void readADC() {
|
|||
//Serial.print(throttle_raw); Serial.print(", "); Serial.print(brake_raw); Serial.print(", ");
|
||||
//Serial.print(throttle_pos); Serial.print(", "); Serial.print(brake_pos); Serial.println();
|
||||
|
||||
|
||||
if (digitalRead(PIN_MODE_SWITCH)) { //pushed in, also high if cable got disconnected
|
||||
throttle_pos/=2;
|
||||
digitalWrite(PIN_MODE_LEDG,LOW); //Green, low is on
|
||||
digitalWrite(PIN_MODE_LEDR,HIGH);
|
||||
speedmode=SPEEDMODE_SLOW;
|
||||
}else{ //button not pushed in
|
||||
digitalWrite(PIN_MODE_LEDG,HIGH);
|
||||
digitalWrite(PIN_MODE_LEDR,LOW); //Red
|
||||
speedmode=SPEEDMODE_NORMAL;
|
||||
}
|
||||
|
||||
|
||||
if (speedmode==SPEEDMODE_SLOW) {
|
||||
throttle_pos/=2;
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
void failChecks() {
|
||||
|
@ -508,4 +520,16 @@ void updateMotorparams( MotorParameter &mp, SerialFeedback &fb) {
|
|||
mp.curR_DC[mp.cur_pos] = -fb.curR_DC;
|
||||
mp.millis=loopmillis;
|
||||
log_update=true;
|
||||
}
|
||||
|
||||
void leds() {
|
||||
digitalWrite(PIN_LED_START,(loopmillis/1000)%2 == 0); //high is on for LED_START
|
||||
|
||||
if (speedmode==SPEEDMODE_SLOW) {
|
||||
digitalWrite(PIN_MODE_LEDG,LOW); //Green, low is on
|
||||
digitalWrite(PIN_MODE_LEDR,HIGH);
|
||||
}else if (speedmode==SPEEDMODE_NORMAL) {
|
||||
digitalWrite(PIN_MODE_LEDG,HIGH);
|
||||
digitalWrite(PIN_MODE_LEDR,LOW); //Red
|
||||
}
|
||||
}
|
Loading…
Reference in New Issue