Arduino Code:
//Code on the actual mobile data logger
//Dino Tinitigan
//Controller
#include <NewSoftSerial.h>
#include <Servo.h>
NewSoftSerial mySerial(2, 3); //Serial Port for Motor Controller
Servo myservo;
//------------------definitions for pololu motor controller---------------
//Trex Jr Motor Controller
#define brkM1 192 //followed by 1 data byte
#define revM1 193 //followed by 1 data byte
#define fwdM1 194 //followed by 1 data byte
#define brkM2 200 //followed by 1 data byte
#define revM2 201 //followed by 1 data byte
#define fwdM2 202 //followed by 1 data byte
#define brkM 208 //followed by 2 data bytes
#define revM 213 //followed by 2 data bytes
#define fwdM 218 //followed by 2 data bytes
#define fM1rM2 214 //followed by 2 data bytes
#define rM1fM2 217 //followed by 2 data bytes
//------------------definitions for pololu motor controller---------------
byte incoming;
byte checksum;
byte prevC;
int pos;
int at = 0;
int lTot;
int rTot;
int temp;
float d;
int hazard = 0;
float vRef = 4.80;
byte type;
int v1 = 100; //speed variables
int v2 = 50; //speed variables
void setup()
{
Serial.begin(19200); //connected to XBee
mySerial.begin(19200); //connected to motor controller
myservo.attach(9); // attaches the servo on pin 9 to the servo object
pinMode(2, INPUT);
pinMode(3, OUTPUT);
pinMode(13, OUTPUT);
pinMode(12, OUTPUT);
myservo.write(90);
}
void loop()
{
if(at==1)
{
automated();
}
//-------------movement commands----------------------//
if(Serial.available())
{
incoming = Serial.read();
if(incoming == 'X')
{
brake(64);
at = 0; //turn automated off
}
else if(incoming == 'W')
{
if(prevC!=incoming)
{
forward(v1);
}
prevC = 'W';
at = 0; //turn automated off
}
else if(incoming == 'S')
{
if(prevC!=incoming)
{
reverse(v1);
}
prevC = 'S';
at = 0; //turn automated off
}
else if(incoming == 'A')
{
if(prevC!=incoming)
{
hardLeft(v1);
}
prevC = 'A';
at = 0; //turn automated off
}
else if(incoming == 'D')
{
if(prevC!=incoming)
{
hardRight(v1);
}
prevC = 'D';
at = 0; //turn automated off
}
else if(incoming == 'Q')
{
if(prevC!=incoming)
{
forwardLeft(v1, v2);
}
prevC = 'Q';
at = 0; //turn automated off
}
else if(incoming == 'E')
{
if(prevC!=incoming)
{
forwardRight(v1, v2);
}
prevC = 'E';
at = 0; //turn automated off
}
else if(incoming == 'Z')
{
if(prevC!=incoming)
{
reverseLeft(v1, v2);
}
prevC = 'Z';
at = 0; //turn automated off
}
else if(incoming == 'C')
{
if(prevC!=incoming)
{
reverseRight(v1, v2);
}
prevC = 'C';
at = 0; //turn automated off
}
else if(incoming == 'U')
{
prevC = 'U';
at = 1; //turn automated ON
}
else
{
brake(64);
at = 0; //turn automated off
}
Serial.flush(); //flush the serial port
}
//-------------movement commands----------------------//
//-------------------Sensor Data----------------------//
else
{
sendLight();
sendTemp();
//delay(50); //delay to prevent overflowing the serial buffer
}
//-------------------Sensor Data----------------------//
}
void forward(int spd)
{
mySerial.print(fwdM, BYTE);
mySerial.print(spd, BYTE);
mySerial.print(spd, BYTE);
}
void reverse(int spd)
{
mySerial.print(revM, BYTE);
mySerial.print(spd, BYTE);
mySerial.print(spd, BYTE);
}
void brake(int spd)
{
mySerial.print(brkM, BYTE);
mySerial.print(spd, BYTE);
mySerial.print(spd, BYTE);
}
void forwardRight(int spd1, int spd2)
{
mySerial.print(fwdM, BYTE);
mySerial.print(spd1, BYTE);
mySerial.print(spd2, BYTE);
}
void forwardLeft(int spd1, int spd2)
{
mySerial.print(fwdM, BYTE);
mySerial.print(spd2, BYTE);
mySerial.print(spd1, BYTE);
}
void reverseRight(int spd1, int spd2)
{
mySerial.print(revM, BYTE);
mySerial.print(spd1, BYTE);
mySerial.print(spd2, BYTE);
}
void reverseLeft(int spd1, int spd2)
{
mySerial.print(revM, BYTE);
mySerial.print(spd2, BYTE);
mySerial.print(spd1, BYTE);
}
void hardRight(int spd)
{
mySerial.print(fM1rM2, BYTE);
mySerial.print(spd, BYTE);
mySerial.print(spd, BYTE);
}
void hardLeft(int spd)
{
mySerial.print(rM1fM2, BYTE);
mySerial.print(spd, BYTE);
mySerial.print(spd, BYTE);
}
void automated() //Automated mode
{
hazard = 0; //default value
forward(v1);
int tot = 0;
for(int i = 0; i<5; i++) //get 5 readings and average it
{
temp = getRange();
tot = tot + temp;
delay(50); //delay to let sensor update with new data
}
temp = tot/5;
if(temp <= 8) //closer than 8 inches
{
sweep();
hazard = 1; //hazard found
}
if(hazard==1)
{
if(lTot > rTot)
{
boolean turning = true;
hardLeft(v1);
delay(1000); //initial turn delay
while(turning)
{
delay(50); //delay to let sensor update with new data
int dt = getRange();
if(dt >= 16)
{
//16 inch clearance found
forward(v1);
turning = false;
}
}
}
else
{
boolean turning = true;
hardRight(v1);
delay(1000); //initial turn delay
while(turning)
{
delay(50); //delay to let sensor update with new data
int dt = getRange();
if(dt >= 16)
{
//16 inch clearance found
forward(v1);
turning = false;
}
}
}//end of else
}//end of if
}//end of automated
void sweep()
{
lTot = 0;
rTot = 0;
brake(64); //make sure vehicle is stoppped before doing a sweep
for(pos = 0; pos < 180; pos += 2) //sweeps from 0 degrees to 180 degrees
{
myservo.write(pos);
delay(50);
temp = getRange();
if(pos>90)
{
lTot = lTot + temp;
}
else
{
rTot = rTot + temp;
}
//transmit data
type = 128; //128=>sonar data
checksum = temp + pos;
Serial.write(255); //header byte
Serial.write(type);
Serial.write(temp); //distance byte
Serial.write(pos); //angle byte
Serial.write(checksum); //checksum byte
}
myservo.write(90); //return to middle;
Serial.flush(); //flush serial port
}
int getRange()
{
//reads analog value from port and returns into a converted value in inches
int z = analogRead(0);
d = z*vRef*1000;
d = d/1024;
d = d/9.8;
z = (int)d;
return z;
}
void headLightsOn()
{
digitalWrite(13, HIGH);
digitalWrite(12, HIGH);
}
void headLightsOff()
{
digitalWrite(13, LOW);
digitalWrite(12, LOW);
}
int getTemp()
{
int totalT = 0;
double temp1;
int prevT;
//calculate initial value
temp1 = analogRead(2);
temp1 = temp1*1000; //millivolts factor
temp1 = temp1/1024; //10-bit range
temp1 = temp1*vRef; //vref
temp1 = (temp1-500)/10; //calculate temperature in Celcius
prevT = (int)temp1;
for(int i = 0; i<10; i++) //get an average of 50 readings to fix noise issue
{
temp1 = analogRead(2);
temp1 = temp1*1000; //millivolts factor
temp1 = temp1/1024; //10-bit range
temp1 = temp1*vRef; //vref
temp1 = (temp1-500)/10; //calculate temperature in Celcius
if(temp1 >= (prevT+5)) //huge fluctuation
{
temp1 = prevT; //keep last value and discard invalid value
}
prevT = temp1;
totalT = totalT + temp1;
delay(5);
}
totalT = totalT/10;
return totalT;
}
void sendTemp()
{
type = 89;
int t1 = getTemp();
checksum = t1;
Serial.write(255); //header byte
Serial.write(type); //ID byte
Serial.write(t1); //data byte
Serial.write(checksum); //checksum byte
}
void sendLight()
{
int lightLevel = analogRead(1);
if(lightLevel <600)
{
headLightsOn();
}
else
{
headLightsOff();
}
type = 23;
lightLevel = lightLevel/4; //make the data fit into a byte
checksum = lightLevel + type;
Serial.write(255); //header byte
Serial.write(type); //ID byte
Serial.write(lightLevel); //data byte
Serial.write(checksum); //checksum byte
}
