356 lines
7.1 KiB
C++
356 lines
7.1 KiB
C++
|
|
/**
|
|
* Arduino Nano (Old Bootloader?)
|
|
* L298N
|
|
**/
|
|
|
|
#include <SoftwareSerial.h>
|
|
|
|
/**
|
|
* CONSTANTS
|
|
**/
|
|
|
|
#define Fpos 9
|
|
#define Fneg 10
|
|
|
|
#define Bpos 5
|
|
#define Bneg 6
|
|
|
|
#define Fsignal 11
|
|
#define Bsignal 12
|
|
|
|
#define Fbutton 3
|
|
#define Bbutton 4
|
|
|
|
const int frameTime = 200; //ms
|
|
const int pullSpeed = 255;
|
|
const int pushSpeed = 125;
|
|
const int buttonPress = 100; //ms
|
|
|
|
/**
|
|
* STATE
|
|
**/
|
|
boolean debug_state = false;
|
|
|
|
const int Buttons[2] = {Fbutton, Bbutton};
|
|
volatile int ButtonState[2] = {1, 1};
|
|
volatile long ButtonTime[2] = {0, 0};
|
|
volatile int ButtonMultiple[2] = {0, 0};
|
|
volatile long buttonTime = 0;
|
|
|
|
const int Fmotor = 0;
|
|
const int Bmotor = 1;
|
|
|
|
volatile int Fspeed = 255;
|
|
volatile int Bspeed = 255;
|
|
|
|
|
|
volatile long Ftime = 0;
|
|
volatile long Btime = 0;
|
|
volatile boolean Frunning = false;
|
|
volatile boolean Brunning = false;
|
|
|
|
volatile long timer = 0;
|
|
|
|
/**
|
|
* SERIAL
|
|
**/
|
|
|
|
volatile char cmdChar = 'z';
|
|
const char cmd_debug = 'd';
|
|
const char cmd_connect = 'i';
|
|
const char cmd_mcopy_identifier = 'm';
|
|
const char cmd_takeup_identifier = 'F';
|
|
const char Fcmd = 'D';
|
|
const char Bcmd = 'F';
|
|
|
|
const int serialDelay = 5;
|
|
|
|
SoftwareSerial softSerial (Fsignal, Bsignal);
|
|
|
|
void setup() {
|
|
Serial.begin(57600);
|
|
Serial.flush();
|
|
Serial.setTimeout(serialDelay);
|
|
|
|
softSerial.begin(9600);
|
|
softSerial.flush();
|
|
softSerial.setTimeout(serialDelay);
|
|
|
|
pinMode(Fpos, OUTPUT);
|
|
pinMode(Fneg, OUTPUT);
|
|
pinMode(Bpos, OUTPUT);
|
|
pinMode(Bneg, OUTPUT);
|
|
|
|
pinMode(Fbutton, INPUT_PULLUP);
|
|
pinMode(Bbutton, INPUT_PULLUP);
|
|
}
|
|
|
|
void loop() {
|
|
timer = millis();
|
|
|
|
if (Serial.available()) {
|
|
cmdChar = (char)Serial.read();
|
|
}
|
|
|
|
if (cmdChar != 'z') {
|
|
cmd(cmdChar, 0);
|
|
}
|
|
|
|
cmdChar = 'z';
|
|
|
|
if (softSerial.available() > 0) {
|
|
cmdChar = (char)softSerial.read();
|
|
}
|
|
|
|
if (cmdChar != 'z') {
|
|
cmd(cmdChar, 1);
|
|
}
|
|
|
|
cmdChar = 'z';
|
|
|
|
btn(Fmotor);
|
|
btn(Bmotor);
|
|
|
|
if (Brunning || Frunning) {
|
|
monitor();
|
|
}
|
|
}
|
|
|
|
void cmd (char which, int source) {
|
|
if (which == Fcmd && !Frunning) {
|
|
forward(source);
|
|
} else if (which == Bcmd && !Brunning) {
|
|
backward(source);
|
|
} else if (which == cmd_debug) {
|
|
debug(source);
|
|
} else if (which == cmd_connect) {
|
|
connect(source);
|
|
} else if (which == cmd_mcopy_identifier) {
|
|
identify(source);
|
|
}
|
|
}
|
|
|
|
void forward (int source) {
|
|
log("forward()");
|
|
if (source == 0) {
|
|
Serial.println(Fcmd);
|
|
} else if (source == 1) {
|
|
softSerial.write(Fcmd);
|
|
}
|
|
Ftime = timer;
|
|
set_speed(Fmotor, pullSpeed);
|
|
set_speed(Bmotor, pushSpeed);
|
|
Frunning = true;
|
|
counter_clockwise(Fmotor);
|
|
counter_clockwise(Bmotor);
|
|
}
|
|
|
|
void backward (int source) {
|
|
log("backward()");
|
|
if (source == 0) {
|
|
Serial.println(Bcmd);
|
|
} else if (source == 1) {
|
|
softSerial.write(Bcmd);
|
|
}
|
|
Btime = timer;
|
|
set_speed(Bmotor, pullSpeed);
|
|
set_speed(Fmotor, pushSpeed);
|
|
Brunning = true;
|
|
clockwise(Bmotor);
|
|
clockwise(Fmotor);
|
|
}
|
|
|
|
void monitor () {
|
|
if (Frunning && timer - Ftime >= frameTime) {
|
|
stop(Fmotor);
|
|
stop(Bmotor);
|
|
}
|
|
if (Brunning && timer - Btime >= frameTime) {
|
|
stop(Bmotor);
|
|
stop(Fmotor);
|
|
}
|
|
}
|
|
|
|
void stop (int motor) {
|
|
brake_motor(motor);
|
|
}
|
|
|
|
//input value from 0 to 255
|
|
void set_speed (int motor, int val){
|
|
if (motor == Fmotor) {
|
|
Fspeed = val;
|
|
} else if (motor == Bmotor) {
|
|
Bspeed = val;
|
|
}
|
|
}
|
|
|
|
/*
|
|
void clockwise_fade (int motor, int startSpeed, int stopSpeed, int ms) {
|
|
int steps = ceil((float) ms / (float) abs(stopSpeed - startSpeed));
|
|
int stepMs = round((float) ms / (float) steps);
|
|
int motorSpeed = startSpeed;
|
|
|
|
if (motor == Fmotor) {
|
|
analogWrite(Fneg, 0);
|
|
} else if (motor == Bmotor) {
|
|
analogWrite(Bneg, 0);
|
|
}
|
|
for (int i = 0; i < steps; i++) {
|
|
if (motor == Fmotor) {
|
|
analogWrite(Fpos, motorSpeed);
|
|
Frunning = true;
|
|
} else if (motor == Bmotor) {
|
|
analogWrite(Bpos, motorSpeed);
|
|
Brunning = true;
|
|
}
|
|
delay(stepMs);
|
|
motorSpeed += stopSpeed > startSpeed ? 1 : -1;
|
|
}
|
|
if (motor == Fmotor) {
|
|
Fspeed = stopSpeed;
|
|
} else if (motor == Bmotor) {
|
|
Bspeed = stopSpeed;
|
|
}
|
|
}
|
|
|
|
void counter_clockwise_fade (int motor, int startSpeed, int stopSpeed, int ms) {
|
|
int steps = ceil((float) ms / (float) abs(stopSpeed - startSpeed));
|
|
int stepMs = round((float) ms / (float) steps);
|
|
int motorSpeed = startSpeed;
|
|
|
|
if (motor == Fmotor) {
|
|
analogWrite(Fpos, 0);
|
|
} else if (motor == Bmotor) {
|
|
analogWrite(Bpos, 0);
|
|
}
|
|
for (int i = 0; i < steps; i++) {
|
|
if (motor == Fmotor) {
|
|
analogWrite(Fneg, motorSpeed);
|
|
Frunning = true;
|
|
} else if (motor == Bmotor) {
|
|
analogWrite(Bneg, motorSpeed);
|
|
Brunning = true;
|
|
}
|
|
delay(stepMs);
|
|
motorSpeed += stopSpeed > startSpeed ? 1 : -1;
|
|
}
|
|
if (motor == Fmotor) {
|
|
Fspeed = stopSpeed;
|
|
} else if (motor == Bmotor) {
|
|
Bspeed = stopSpeed;
|
|
}
|
|
}
|
|
*/
|
|
void clockwise (int motor){
|
|
if (motor == Fmotor) {
|
|
analogWrite(Fpos, Fspeed);
|
|
analogWrite(Fneg, 0);
|
|
}
|
|
if (motor == Bmotor) {
|
|
analogWrite(Bpos, Bspeed);
|
|
analogWrite(Bneg, 0);
|
|
}
|
|
}
|
|
|
|
void counter_clockwise (int motor){
|
|
if (motor == Fmotor) {
|
|
analogWrite(Fpos, 0);
|
|
analogWrite(Fneg, Fspeed);
|
|
}
|
|
if (motor == Bmotor) {
|
|
analogWrite(Bpos, 0);
|
|
analogWrite(Bneg, Bspeed);
|
|
}
|
|
}
|
|
|
|
void brake (){
|
|
if (Frunning) {
|
|
brake_motor(Fmotor);
|
|
}
|
|
if (Brunning) {
|
|
brake_motor(Bmotor);
|
|
}
|
|
}
|
|
|
|
void brake_motor (int motor) {
|
|
if (motor == Fmotor) {
|
|
digitalWrite(Fpos, LOW);
|
|
digitalWrite(Fneg, LOW);
|
|
Frunning = false;
|
|
} else if (motor == Bmotor) {
|
|
digitalWrite(Bpos, LOW);
|
|
digitalWrite(Bneg, LOW);
|
|
Brunning = false;
|
|
}
|
|
}
|
|
|
|
/* ------------------------------------------------
|
|
* Reads the state of a specific button and compares
|
|
* it to a stored value in the button_state array.
|
|
* If the value is different than what is stored,
|
|
* check if button is pressed and store new timer value,
|
|
* if button is released, compare current time to the stored
|
|
* time value and pass that to the button_end function.
|
|
* ------------------------------------------------*/
|
|
void btn (int index) {
|
|
int val = digitalRead(Buttons[index]);
|
|
if (val != ButtonState[index]) {
|
|
if (val == LOW) { // pressed
|
|
ButtonTime[index] = timer;
|
|
ButtonMultiple[index] = -1;
|
|
}
|
|
}
|
|
if (val == LOW) {
|
|
buttonTime = timer - ButtonTime[index];
|
|
btnCmd(index, buttonTime);
|
|
}
|
|
ButtonState[index] = val;
|
|
}
|
|
|
|
void btnCmd (int index, int time) {
|
|
int multiple = floor(time / buttonPress);
|
|
if (multiple > ButtonMultiple[index]) {
|
|
if (index == Fmotor) {
|
|
cmd(Fcmd, 0);
|
|
} else if (index == Bmotor) {
|
|
cmd(Bcmd, 0);
|
|
}
|
|
ButtonMultiple[index] = multiple;
|
|
}
|
|
}
|
|
|
|
void connect (int source) {
|
|
if (source == 0) {
|
|
Serial.println(cmd_connect);
|
|
} else if (source == 1) {
|
|
softSerial.write(cmd_connect);
|
|
}
|
|
log("connect()");
|
|
}
|
|
|
|
void identify (int source) {
|
|
if (source == 0) {
|
|
Serial.println(cmd_takeup_identifier);
|
|
} else if (source == 1) {
|
|
softSerial.write(cmd_takeup_identifier);
|
|
}
|
|
log("identify()");
|
|
}
|
|
|
|
void debug (int source) {
|
|
debug_state = true;
|
|
if (source == 0) {
|
|
Serial.println(cmd_debug);
|
|
} else if (source == 1) {
|
|
softSerial.write(cmd_debug);
|
|
}
|
|
log("debugging enabled");
|
|
}
|
|
|
|
void log (String msg) {
|
|
if (debug_state) {
|
|
Serial.println(msg);
|
|
}
|
|
}
|