#define D_FRONT 1
#define D_STOP 0
#define D_BACK 2
//pocet cyklu
long cycle;
//Kalibracni data
int track_left_minspeed = 32;
int track_right_minspeed = 32;
int track_left_maxspeed = 255;
int track_right_maxspeed = 100;
//smery otaceni pasu
int track_left = D_STOP;
int track_right = D_STOP;
//rychlosti... pokud se rovnaji, snazime se jet podle odometrie rovne.
int track_left_speed = 0;
int track_right_speed = 0;
//zastavili sme kvuli prekazce?
int ir_proximity = 0;
class Smoother
{
public:
Smoother(int velikost);
Smoother();
void nastav(int velikost);
void zapis(int kolik);
int precti();
private:
int pvelikost;
int velikost;
int zaznamy[20];
int index;
int soucet;
};
Smoother::Smoother(){
nastav(10);
}
Smoother::Smoother(int velikost){
nastav(velikost);
}
void Smoother::nastav(int pvelikost){
velikost = pvelikost;
if (velikost > 20)
velikost = 20;
for (index = 0; index < velikost; index ++)
zaznamy[index] = 0;
index = 0;
soucet = 0;
Serial.println("nastaveno");
}
void Smoother::zapis(int kolik){
soucet = soucet - zaznamy[index];
zaznamy[index] = kolik;
soucet = soucet + kolik;
index++;
if (index >= velikost)
index = 0;
}
int Smoother::precti(){
return soucet/velikost;
}
//odometricky bordel
class Odometr
{
public:
Odometr(int apin);
int Cti(int mydir, int otherdir);
long total_distance;
private:
int last_color, color, last_reading, reading, pin;
// int mydir, otherdir;
long last_drive_cmd_distance;
long last_odo_cmd_distance;
int _zjisti_barvu(int reading);
};
Odometr::Odometr(int apin){
pin = apin;
//Konstruktor. Nacte nejakou hodnotu abysme nezacinali s jednim krokem
last_color, color, last_reading, reading = 0;
last_reading = analogRead(pin);
last_color = _zjisti_barvu(last_reading);
}
int Odometr::_zjisti_barvu(int reading){
if (reading+last_reading < 800 ){
//cerna
color = 1;
}
else
if (reading+last_reading > 1100 ){
//bila
color = 2;
}
else {
color = 0;
}
last_reading = reading;
return color;
}
int Odometr::Cti(int mydir, int otherdir){
//Precte hodnotu ze senzoru. Pokud se nezmenila vrati nulu
//pokud se zmenila barva tak vrati +-1 podle smeru.
last_color = color;
reading = analogRead(pin);
color = _zjisti_barvu(reading);
if (last_color != color and color != 0) {
//...zmenila se barva na kolecku
//pokud je pas ve stavu D_STOP tak bereme smer z opacneho pasu,
//protoze nas tahne. Jinak nas asi nekdo postrkuje, takze nezname smer.
if (mydir == D_STOP) {
mydir = otherdir;
}
if (mydir == D_STOP) {
return 0;
}
if (mydir == D_FRONT) {
return 1;
}
else {
return -1;
}
}
return 0;
}
Odometr odometr_left = Odometr(2);
Odometr odometr_right = Odometr(1);
int odo_stopper = 0;
Smoother ir_left = Smoother();
Smoother ir_right = Smoother();
//zmackl nekdo cervene tlacitko?
int infire = false;
//mereni na jednotlivejch pasech
long odo_left = 0;
long odo_right = 0;
//vracime zmeny od posledniho mereni.
long last_odo_left = 0;
long last_odo_right = 0;
//odo posledniho Dxxxx prikazu
long odo_left_this_cmd = 0;
long odo_right_this_cmd = 0;
long odo_left_last_adj = 0;
long odo_right_last_adj = 0;
//We dont want to do any Dxxxx command for more than 1 second...
unsigned long cmd_time = 0;
void setup() {
// initialize the digital pin as an output:
pinMode(2, OUTPUT); //l low BK
pinMode(3, OUTPUT); //l low RD
pinMode(4, OUTPUT); //r low RD
pinMode(5, OUTPUT); //r low BK
pinMode(22, OUTPUT); //l high BK
pinMode(23, OUTPUT); //l high RD
pinMode(24, OUTPUT); //r high RD
pinMode(25, OUTPUT); //r high BK
pinMode(26, INPUT); //big red button
digitalWrite(2, LOW);
digitalWrite(3, LOW);
digitalWrite(4, LOW);
digitalWrite(5, LOW);
digitalWrite(22, LOW);
digitalWrite(23, LOW);
digitalWrite(24, LOW);
digitalWrite(25, LOW);
Serial.begin(9600);
}
int moving(){
return ((track_right != D_STOP) or (track_left != D_STOP));
}
void leva_vpred(int kolik) {
if (!infire){
if (track_left != D_FRONT) {
digitalWrite(2, LOW);
digitalWrite(23, LOW);
delay(10);
digitalWrite(22, HIGH);
}
analogWrite(3, kolik);
track_left = D_FRONT;
}
}
void leva_vzad(int kolik) {
if (!infire){
if (track_left != D_BACK) {
digitalWrite(3, LOW);
digitalWrite(22, LOW);
delay(10);
digitalWrite(23, HIGH);
}
analogWrite(2, kolik);
track_left = D_BACK;
}
}
void prava_vpred(int kolik) {
if (!infire){
if (track_right != D_FRONT) {
digitalWrite(5, LOW);
digitalWrite(24, LOW);
delay(10);
digitalWrite(25, HIGH);
}
analogWrite(4, kolik);
track_right = D_FRONT;
}
}
void prava_vzad(int kolik) {
if (!infire){
if (track_right != D_BACK) {
digitalWrite(4, LOW);
digitalWrite(25, LOW);
delay(10);
digitalWrite(24, HIGH);
}
analogWrite(5, kolik);
track_right = D_BACK;
}
}
void prava_stop(){
digitalWrite(24, LOW);
digitalWrite(25, LOW);
delay (10);
digitalWrite(4, HIGH);
digitalWrite(5, HIGH);
track_right = D_STOP;
}
void leva_stop(){
digitalWrite(22, LOW);
digitalWrite(23, LOW);
delay (10);
digitalWrite(2, HIGH);
digitalWrite(3, HIGH);
track_left = D_STOP;
}
void b_stop(){
odo_stopper = 0;
prava_stop();
leva_stop();
}
void brb_read(){
if (digitalRead(26) == 1) {
infire = true;
b_stop();
}
}
void return_ok(){
if (infire) {
Serial.println("ON FIRE");
}
else
if (ir_proximity) {
Serial.println("HALTED");
ir_proximity = false;
}
else {
Serial.println("OK");
}
}
void read_serial(){
if (Serial.available()){
//precteni a narobeni prikazu ze seriaku
char incomingByte = Serial.read();
switch (incomingByte){
case 'N':
{
infire = false;
return_ok();
break;
}
case 'S':
case 's':
{
b_stop();
return_ok();
break;
}
case 'O':
{
Serial.print(odo_left-last_odo_left, DEC);
Serial.print(",");
Serial.print(odo_right-last_odo_right, DEC);
Serial.println("");
last_odo_left = odo_left;
last_odo_right = odo_right;
break;
}
case 'P':
{
Serial.print(ir_left.precti(), DEC);
Serial.print(",");
Serial.print(ir_right.precti(), DEC);
Serial.println("");
break;
}
case 'F':
{
int osdelta = 0;
odo_left_this_cmd = odo_left;
odo_right_this_cmd = odo_right;
delay(10);
while (Serial.available()){
odo_stopper = odo_stopper * 10;
osdelta = Serial.read();
if (osdelta > 58 or osdelta < 48) {
break;
} else {
odo_stopper = odo_stopper + osdelta - 48;
}
}
return_ok();
break;
};
case 'D':
{
delay(10);
odo_left_this_cmd = odo_left;
odo_right_this_cmd = odo_right;
char l_kam = Serial.read();
int l_kolik = Serial.read() - 48;
char r_kam = Serial.read();
int r_kolik = Serial.read() - 48;
if (r_kolik == 0) {
r_kam = 'f';
}
if (l_kolik == 0) {
l_kam = 'f';
leva_stop();
}
else {
if (l_kam == 'F' || l_kam == 'f') {
if (l_kam == r_kam) {
leva_vpred(map(l_kolik, 0,9, track_left_minspeed, track_left_maxspeed));
}
else {
leva_vpred(map(l_kolik, 0,9, track_left_minspeed, 255));
}
track_left_speed = l_kolik;
}
else if (l_kam == 'R') {
if (l_kam == r_kam) {
leva_vzad(map(l_kolik, 0,9, track_left_minspeed, track_left_maxspeed));
}
else {
leva_vzad(map(l_kolik, 0,9, track_left_minspeed, 255));
}
track_left_speed = - l_kolik;
}
else {
b_stop();
Serial.println("Err L");
return;
}
}
if (r_kolik == 0) {
prava_stop();
}
else {
if (r_kam == 'F' || r_kam == 'f') {
if (l_kam == r_kam) {
prava_vpred(map(r_kolik, 0,9, track_right_minspeed, track_right_maxspeed));
}
else {
prava_vpred(map(r_kolik, 0,9, track_right_minspeed, 255));
}
track_right_speed = r_kolik;
}
else if (r_kam == 'R') {
if (l_kam == r_kam) {
prava_vzad(map(r_kolik, 0,9, track_right_minspeed, track_right_maxspeed));
}
else {
prava_vzad(map(r_kolik, 0,9, track_right_minspeed, 255));
}
track_right_speed = -r_kolik;
}
else {
b_stop();
Serial.println("Err R");
return;
}
}
cmd_time = millis();
return_ok();
break;
}
case 'w':
case 'W':
{
Serial.println("time;speedl,speedr;mapped_speedl,mapped_speedr;odo_left,odo_right;odo_left_this_cmd,odo_right_this_cmd;track_left,track_right;max_speed_l,max_speed_r;cycle;infire");
Serial.print(millis());
Serial.print(";");
Serial.print(track_left_speed);
Serial.print(",");
Serial.print(track_right_speed);
Serial.print(";");
Serial.print(map(abs(track_left_speed), 1,9, track_left_minspeed, track_left_maxspeed));
Serial.print(",");
Serial.print(map(abs(track_right_speed), 1,9, track_right_minspeed, track_right_maxspeed));
Serial.print(";");
Serial.print(odo_left);
Serial.print(",");
Serial.print(odo_right);
Serial.print(";");
Serial.print(odo_left_this_cmd);
Serial.print(",");
Serial.print(odo_right_this_cmd);
Serial.print(";");
Serial.print(track_left);
Serial.print(",");
Serial.print(track_right);
Serial.print(";");
Serial.print(track_left_maxspeed);
Serial.print(",");
Serial.print(track_right_maxspeed);
Serial.print(";");
Serial.print(cycle);
Serial.print(";");
Serial.print(infire);
Serial.println("");
}
}
}
}
void read_odometry(){
int debug = false;
odo_left = odo_left + odometr_left.Cti(track_left, track_right);
odo_right = odo_right + odometr_right.Cti(track_right, track_left);
//kompenzace rozdilne rychlosti pasu. Provadi se jenom pokud jedeme rovne
//tak nejak po kazdych 10 snimnutich zmenach.
if (track_left_speed == track_right_speed) {
if ((odo_left-odo_left_this_cmd-odo_left_last_adj) > 20){
//rozdil v poctu zaznamenanych zmen barvy. Pokud je > 0 je levy pas rychlejsi
int sdiff = (odo_left-odo_left_this_cmd-odo_left_last_adj) - (odo_right-odo_right_this_cmd-odo_right_last_adj);
if (sdiff > 2) {
if (debug) Serial.println("Adjusting +");
//zrychli pravy pas, pokud to nejde tak spomal levy.
if (track_right_maxspeed < 255) {
track_right_maxspeed += 10;
if (track_right_maxspeed > 255) {
track_right_maxspeed = 255;
}
}
else {
track_left_maxspeed -= 10;
if (track_left_maxspeed < 80) {
track_left_maxspeed = 80;
}
}
}
else if (sdiff < 2) {
if (debug) Serial.println("Adjusting -");
if (track_left_maxspeed < 255) {
track_left_maxspeed += 10;
if (track_left_maxspeed > 255) {
track_left_maxspeed = 255;
}
}
else {
track_right_maxspeed -= 10;
if (track_right_maxspeed < 80) {
track_right_maxspeed = 80;
}
}
}
odo_left_last_adj = odo_left-odo_left_this_cmd;
odo_right_last_adj = odo_right-odo_right_this_cmd;
}
}
}
// the loop() method runs over and over again,
// as long as the Arduino has power
void loop()
{
cycle++;
//check big red button
brb_read();
// read serial port and execute commands
read_serial();
// read odometry and adjust speed limits when it makes sense
read_odometry();
if (odo_stopper > 0) {
if (odo_stopper < max( abs(odo_left-odo_left_this_cmd), abs(odo_right-odo_right_this_cmd)) ){
b_stop();
Serial.println("THERE");
odo_stopper = 0;
}
}
//check proximity sensors
ir_left.zapis(analogRead(3));
ir_right.zapis(analogRead(4));
if ((ir_left.precti() > 350) or (ir_right.precti() > 350)) {
//prekazka pred ir senzory
if (not ir_proximity)
Serial.println("HALTED");
b_stop();
ir_proximity = true;
}
}