James Plager
ECE4180 Final Project
Dependencies: LSM9DS1_Library Motor mbed-rtos mbed
Fork of
IMURoomba4_ThrowSumMo
by 
James Plager
Diff: main.cpp
- Revision:
- 3:17113c72186e
- Parent:
- 2:8887d13f967a
- Child:
- 4:63e69557142e
--- a/main.cpp Wed May 03 02:19:12 2017 +0000
+++ b/main.cpp Thu May 04 19:25:25 2017 +0000
@@ -15,7 +15,7 @@
Serial pc(USBTX, USBRX);
//RawSerial pc(USBTX, USBRX);
-Serial dev(p28,p27); //
+Serial dev(p28,p27); //
//RawSerial dev(p28,p27); //tx, rx
DigitalOut myled(LED1);
DigitalOut led2(LED2);
@@ -28,19 +28,19 @@
Motor Left(p21, p14, p13); // green wires. pwm, fwd, rev, add ", 1" for braking
Motor Right(p22, p12, p11); // red wires
// Speaker out
-AnalogOut DACout(p18); //must(?) be p18
+//AnalogOut DACout(p18); //must(?) be p18
//SDFileSystem sd(p5, p6, p7, p8, "sd"); //SD card
Thread thread1;
-Thread thread2;
-Mutex BTmutex; //mutex for send/recv data on Bluetooth
+//Thread thread2;
+//Mutex BTmutex; //mutex for send/recv data on Bluetooth
Mutex mutex; //other mutex for global resources
//Globals
-float throttle = 0.3;
+float throttle = 0.5;
float currIR1;
float currIR2;
-float origHeading;
-float heading;
+//float origHeading;
+//float heading;
// Calculate pitch, roll, and heading.
// Pitch/roll calculations taken from this app note:
@@ -48,36 +48,38 @@
// Heading calculations taken from this app note:
// http://www51.honeywell.com/aero/common/documents/myaerospacecatalog-documents/Defense_Brochures-documents/Magnetic__Literature_Application_notes-documents/AN203_Compass_Heading_Using_Magnetometers.pdf
void printAttitude(float ax, float ay, float az, float mx, float my, float mz)
-{ //entire subroutine is BTmutexed already
+{
+ //entire subroutine is BTmutexed already
float roll = atan2(ay, az);
float pitch = atan2(-ax, sqrt(ay * ay + az * az));
// touchy trig stuff to use arctan to get compass heading (scale is 0..360)
mx = -mx;
- //float heading; //turning to global
+ float heading; //was global
if (my == 0.0) {
- mutex.lock();
+ //mutex.lock(); //heading isn't global mutexes not needed
heading = (mx < 0.0) ? 180.0 : 0.0;
- mutex.unlock();
- }
- else {
- mutex.lock();
+ //mutex.unlock();
+ } else {
+ //mutex.lock();
heading = atan2(mx, my)*360.0/(2.0*PI);
- mutex.unlock();
+ //mutex.unlock();
}
//pc.printf("heading atan=%f \n\r",heading);
- mutex.lock();
+ //mutex.lock();
heading -= DECLINATION; //correct for geo location
if(heading>180.0) heading = heading - 360.0;
else if(heading<-180.0) heading = 360.0 + heading;
else if(heading<0.0) heading = 360.0 + heading;
- mutex.unlock();
+ //mutex.unlock();
// Convert everything from radians to degrees:
//heading *= 180.0 / PI;
pitch *= 180.0 / PI;
roll *= 180.0 / PI;
+ mutex.lock();
//~pc.printf("Pitch: %f, Roll: %f degress\n\r",pitch,roll);
//~pc.printf("Magnetic Heading: %f degress\n\r",heading);
- dev.printf("Magnetic Heading: %f degrees\n\r",heading);
+ //dev.printf("Magnetic Heading: %f degrees\n\r",heading);
+ mutex.unlock();
}
/*
@@ -96,46 +98,47 @@
}
}*/
- // Driving Methods
-void forward(float speed){
+// Driving Methods
+void forward(float speed)
+{
Left.speed(speed);
- Right.speed(speed);
+ Right.speed(speed);
}
-void reverse(float speed){
+void reverse(float speed)
+{
Left.speed(-speed);
Right.speed(-speed);
}
-void turnRight(float speed){
+void turnRight(float speed)
+{
Left.speed(speed);
Right.speed(-speed);
//wait(0.7);
}
-void turnLeft(float speed){
+void turnLeft(float speed)
+{
Left.speed(-speed);
Right.speed(speed);
//wait(0.7);
}
-void stop(){
+void stop()
+{
Left.speed(0.0);
Right.speed(0.0);
}
-void IMU(){
+void IMU()
+{
//IMU setup
LSM9DS1 IMU(p9, p10, 0xD6, 0x3C); // this executes. Pins are correct. Changing them causes fault
- IMU.begin();
- if (!IMU.begin()) {
+ IMU.begin();
+ if (!IMU.begin()) {
led2=1;
pc.printf("Failed to communicate with LSM9DS1.\n");
}
IMU.calibrate(1);
IMU.calibrateMag(0);
- /*
- //bluetooth setup
- pc.baud(9600);
- dev.baud(9600); */
- /*pc.attach(&pc_recv, Serial::RxIrq);
- dev.attach(&dev_recv, Serial::RxIrq);*/
+
while(1) {
//myled = 1;
while(!IMU.magAvailable(X_AXIS));
@@ -145,107 +148,117 @@
IMU.readAccel();
while(!IMU.gyroAvailable());
IMU.readGyro();
- BTmutex.lock();
- pc.puts(" X axis Y axis Z axis\n\r");
+ //mutex.lock(); //changed from BTmutex
+ //pc.puts(" X axis Y axis Z axis\n\r");
//dev.puts(" X axis Y axis Z axis\n\r");
- pc.printf("gyro: %9f %9f %9f in deg/s\n\r", IMU.calcGyro(IMU.gx), IMU.calcGyro(IMU.gy), IMU.calcGyro(IMU.gz));
- pc.printf("accel: %9f %9f %9f in Gs\n\r", IMU.calcAccel(IMU.ax), IMU.calcAccel(IMU.ay), IMU.calcAccel(IMU.az));
- pc.printf("mag: %9f %9f %9f in gauss\n\r", IMU.calcMag(IMU.mx), IMU.calcMag(IMU.my), IMU.calcMag(IMU.mz));
+ //pc.printf("gyro: %9f %9f %9f in deg/s\n\r", IMU.calcGyro(IMU.gx), IMU.calcGyro(IMU.gy), IMU.calcGyro(IMU.gz));
+ //pc.printf("accel: %9f %9f %9f in Gs\n\r", IMU.calcAccel(IMU.ax), IMU.calcAccel(IMU.ay), IMU.calcAccel(IMU.az));
+ //pc.printf("mag: %9f %9f %9f in gauss\n\r", IMU.calcMag(IMU.mx), IMU.calcMag(IMU.my), IMU.calcMag(IMU.mz));
//dev.printf("mag: %9f %9f %9f in gauss\n\r", IMU.calcMag(IMU.mx), IMU.calcMag(IMU.my), IMU.calcMag(IMU.mz));
printAttitude(IMU.calcAccel(IMU.ax), IMU.calcAccel(IMU.ay), IMU.calcAccel(IMU.az), IMU.calcMag(IMU.mx),
IMU.calcMag(IMU.my), IMU.calcMag(IMU.mz));
- BTmutex.unlock();
+ //mutex.unlock(); //changed from BTmutex
//myled = 1;
wait(0.5);
//myled = 0;
- wait(0.5);
- }
+ wait(0.5);
+ }
}
-void avoidObstacle(){
- currIR1 = IR1; //get IR readings
- currIR2 = IR2;
- //if(currIR1 > 0.8){
+void avoidObstacle()
+{
+ //currIR1 = IR1; //get IR readings - already received from main thread that initially decided to call avoidObstacle()
+ //currIR2 = IR2;
stop();
Thread::wait(300);
- BTmutex.lock();
+ //BTmutex.lock();
//dev.printf("Collision Detected!\n\r");
- BTmutex.unlock();
+ //BTmutex.unlock();
//dev.printf("Turning Left...\n\r");
turnLeft(0.4); //turn 90deg
Thread::wait(1000); //time to turn estimate
- /*
- while(IR2 < 0.7){ //turn left until IR2 detects an object.
- currIR2 = IR2;
- Thread::wait(300);
- } */
stop();
- Thread::wait(1000);
+ Thread::wait(500);
// turn should be complete. Drive until obstacle passed on right, then turn right again
//BTmutex.lock();
//dev.printf("Driving past obstacle.\n\r");
//BTmutex.unlock();
forward(throttle);
bool objOnRight = true;
- while(objOnRight){
- currIR1 = IR1;
+ while(objOnRight) {
+ mutex.lock();
+ dev.printf("Avoiding Obstacles...\n\r");
+ currIR1 = IR1; //must keep scanning IR readers to know when object is cleared
currIR2 = IR2;
- if(currIR2 < 0.7){ objOnRight = false;} //if IR2 drops below threshold, obstacle passed. Break out of loop
- Thread::wait(250);
+ dev.printf(" IR1 Reading IR2 Reading\n\r %f %f\n\r", currIR1, currIR2);
+ mutex.unlock();
+ if(currIR2 < 0.7) {
+ objOnRight = false; //if IR2 drops below threshold, obstacle passed. Break out of loop
+
+ wait(0.5); //give robot time to drive past object
+ }
+ if(currIR1 > 0.8){ // don;t crash to anything in front
+ stop();
+ myled=led2=led3=led4=1;
+ }
+ //Thread::wait(1250);
}
stop();
Thread::wait(250);
- BTmutex.lock();
+ //BTmutex.lock();
//dev.printf("Object passed. Turning right...\n\r");
- turnRight(0.4); // turn 90deg
+ turnRight(0.5); // turn 90deg
Thread::wait(1000); //time to turn estimate
stop();
Thread::wait(1000);
- forward(throttle);
-
- //}
+ forward(throttle);
}
-void defaultDrive(){ //default behavior for robot
+/*
+void defaultDrive() //default behavior for robot //moved to main instead of being a thread
+{
//Drive forward until object detected. Stop, turn left, then drive until IR2 says path is clear, then turn right to go around object.
forward(throttle);
- while(1){
+ while(1) {
+ myled=1;
//update current IR readings
currIR1 = IR1;
currIR2 = IR2;
BTmutex.lock(); //prevent race conditions in BT dataoutput
- //dev.puts(" Front IR reading Right IR reading\n\r"); // print IR readings over BT
+ //dev.puts(" Front IR reading Right IR reading\n\r"); // print IR readings over BT
//dev.printf(" %2f %2f\n\r", currIR1, currIR2);
+ pc.puts(" Front IR reading Right IR reading\n\r"); // print IR readings over BT
+ pc.printf(" %2f %2f\n\r", currIR1, currIR2);
+
BTmutex.unlock();
// Forward collision handling code block
if(currIR1 > 0.8) { // 0.85 is threshold for collision
+ led3=1;
avoidObstacle(); // steer around obstacle when detected
-
- /*PICK UP FROM HERE
- Implement logic to control two scenarios:
- 1) Roomba has detected obstacle in front, but Right is clear.
- */
-
- }
+ led3=0;
+ }
Thread::wait(400); // for debug. IR polling too quick and floods output terminal
+ wait(0.4);
+ myled=0;
}
}
+*/
-void manualMode(){
+/*
+void manualMode() // also moved to main
+{
bool on = true;
char temp;
- while(on){
+ while(on) {
temp = dev.getc();
- if(temp == 'A'){ // reset command
+ if(temp == 'A') { // reset command
on = false;
- }
- else if(temp=='U'){
+ } else if(temp=='U') {
led2=led3=1;
forward(throttle);
wait(1);
led2=led3=0;
- }
- else if(temp=='L'){ // turn left
+ } else if(temp=='L') { // turn left
myled=led2=1; //debug
stop();
wait(0.3);
@@ -255,8 +268,7 @@
wait(0.3);
forward(throttle);
myled=led2=0; //debug
- }
- else if(temp=='R'){ // turn right
+ } else if(temp=='R') { // turn right
led3=led4=1;
stop();
wait(0.3);
@@ -266,8 +278,7 @@
wait(0.3);
forward(throttle);
led3=led4=0;
- }
- else if(temp=='X'){ // halt/brake command
+ } else if(temp=='X') { // halt/brake command
stop();
}
//myled=1;
@@ -276,21 +287,31 @@
//wait(0.5);
}
}
+*/
+
+/*
+void updateIRs()
+{
+ mutex.lock();
+ currIR1 = IR1; //must keep scanning IR readers to know when object is cleared
+ currIR2 = IR2;
+ mutex.unlock();
+}*/
int main()
{
//bluetooth setup
pc.baud(9600);
dev.baud(9600);
- //wait to recv start command
-
- for(int i=0; i<3;i++){ //temp delay for a few sec
+ //wait to recv start command or time delay
+ for(int i=0; i<3; i++) { //temp delay for a few sec
myled=led2=led3=led4=1;
wait(0.5);
myled=led2=led3=led4=0;
wait(0.5);
- }
+ }
thread1.start(IMU); // start the IMU thread
+ char state = 'D'; //Roomba's drive state
char temp;
/*
while(1){ //robot will receive a char from GUI signalling time to start
@@ -300,26 +321,104 @@
if (temp == 'B'){
break;
}
-
if(led2 == 0) led2 = 1;
else {led2 = 0;}
wait(0.25);
- }
+ }
*/
led3=0;
- thread2.start(defaultDrive);
- while(1){
- temp = dev.getc();
- if(temp == 'M'){
- led4=1;
- stop();
- thread2.terminate(); //stop default drive
- manualMode(); //switch to manual control
- //once manualMode is exited, return to default
- led4=0;
- thread2.start(defaultDrive);
+ //thread2.start(defaultDrive); default drive inserted into main while
+ while(1) {
+ //Drive forward until object detected. Stop, turn left, then drive until IR2 says path is clear, then turn right to go around object.
+ forward(throttle);
+ while(state == 'D') { //default drive
+ myled=1;
+ //update current IR readings
+ //mutex.lock();//IR readings included in mutex since they are shared global variables
+ currIR1 = IR1;
+ currIR2 = IR2;
+ mutex.lock(); //prevent race conditions in BT dataoutput //changed from BTmutex
+ dev.puts(" Front IR reading Right IR reading\n\r"); // print IR readings over BT
+ dev.printf(" %2f %2f\n\r", currIR1, currIR2);
+ //pc.puts(" Front IR reading Right IR reading\n\r"); // print IR readings over serial
+ //pc.printf(" %2f %2f\n\r", currIR1, currIR2);
+ mutex.unlock(); // changed from BTmutex
+
+ // Forward collision handling code block
+ if(currIR1 > 0.8) { // 0.85 is threshold for collision
+ led3=1;
+ avoidObstacle(); // steer around obstacle when detected
+ led3=0;
+ }
+ Thread::wait(400); // for debug. IR polling too quick and floods output terminal
+ wait(0.4);
+ myled=0;
+
+ //was already ITT
+ if (dev.readable()) {
+ mutex.lock();
+ temp = dev.getc();
+ mutex.unlock();
+ }
+ if(temp == 'M') {
+ led4=1;
+ stop();
+ //thread2.terminate(); //stop default drive
+ //manualMode(); //switch to manual control
+ /*
+ while(1) {
+ temp = dev.getc();
+ if(temp=='U') {
+ led2=1;
+ }
+ } */
+ //once manualMode is exited, return to default
+ led4=0;
+ //thread2.start(defaultDrive);
+ state = 'M';
+ }
+ }
+
+ while(state == 'M') {
+ if (dev.readable()){
+ mutex.lock();
+ temp = dev.getc();
+ mutex.unlock();
+ }
+ if(temp == 'A') { // reset command
+ state = 'D';
+ } else if(temp=='U') {
+ led2=led3=1;
+ forward(throttle);
+ wait(1);
+ led2=led3=0;
+ } else if(temp=='L') { // turn left
+ myled=led2=1; //debug
+ stop();
+ wait(0.5);
+ turnLeft(0.4);
+ wait(1);
+ stop();
+ wait(0.5);
+ forward(throttle);
+ myled=led2=0; //debug
+ } else if(temp=='R') { // turn right
+ led3=led4=1;
+ stop();
+ wait(0.3);
+ turnRight(0.5);
+ wait(0.6);
+ stop();
+ wait(0.3);
+ forward(throttle);
+ led3=led4=0;
+ } else if(temp=='X') { // halt/brake command
+ stop();
+ }
+ //myled=1;
+ //wait(0.5);
+ //myled=0;
+ //wait(0.5);
}
}
-
-}
-
+}
\ No newline at end of file