Diff: main.cpp

Revision:

3:f41855c51aaf

Parent:

1:4a490de4c2a4

--- a/main.cpp	Thu Nov 03 14:16:52 2016 +0000
+++ b/main.cpp	Fri Nov 04 00:11:18 2016 +0000
@@ -10,7 +10,7 @@
 #define DECLINATION -4.94 // Declination (degrees) in Atlanta,GA.
 
 Motor LeftM(p21, p22, p23,1); // pwm, fwd, rev
-Motor RightM(p26, p25, p24,1);
+Motor RightM(p26, p27, p24,1);
 
 InterruptIn rhes(p15);
 InterruptIn lhes(p16);
@@ -26,8 +26,12 @@
 
 LSM9DS1 IMU(p9, p10, 0xD6, 0x3C);
 
+PwmOut speaker(p25);
+
 int countl = 0, countr = 0;
-    
+int xx=0,yy=0;
+
+float note1 = 1568.0, note2 = 1396.9, note3 = 1244.5;
 // Calculate pitch, roll, and heading.
 // Pitch/roll calculations taken from this app note:
 // http://cache.freescale.com/files/sensors/doc/app_note/AN3461.pdf?fpsp=1
@@ -66,6 +70,14 @@
 
 void move(float dist, int dir = 1){
     led1 = 1;
+    
+    speaker.period(1.0/note1);
+    speaker =0.25;
+    wait(0.3);
+    speaker.period(1.0/note2);
+    wait(0.3);
+    speaker=0.0;
+    
     leftPid.setInputLimits(0, 1000);
     leftPid.setOutputLimits(0.0, 0.9);
     leftPid.setMode(AUTO_MODE);
@@ -110,6 +122,14 @@
 
 void turn(int direction){
     led2 = 1;
+    
+    speaker.period(1.0/note2);
+    speaker =0.25;
+    wait(0.3);
+    speaker.period(1.0/note1);
+    wait(0.3);
+    speaker=0.0;
+    
     float head = 0, oldHead = 0,newHead = 0;
     oldHead = getHead();
     head = oldHead;
@@ -147,7 +167,29 @@
     led2 = 0;
     countl = 0;
     countr = 0;
-    wait(1);
+    wait(0.5);
+}
+
+
+void coord(int x, int y){
+    if (y>yy)
+        move((y-yy)*250,1);
+    else if (y<yy)
+        move((yy-y)*250,-1);
+        
+    if (x>xx){
+        turn(1);
+        move((x-xx)*250,1);
+        turn(-1);
+    }
+    else if (x<xx){
+        turn(-1);
+        move((xx-x)*250,1);
+        turn(1);
+    }
+    
+    xx=x;
+    yy=y;
 }
 
 int main()
@@ -156,6 +198,8 @@
     rhes.mode(PullUp);
     lhes.rise(&leftM_count);
     rhes.rise(&rightM_count);
+    xx=0;
+    yy=0;
     IMU.begin();
     if (!IMU.begin()) {
         led1 = 1;
@@ -168,27 +212,20 @@
     IMU.calibrateMag(0);
     led3 = 0;
     wait(2);
-    /*
-    while (1){
-        pc.printf("Heading: %f\n\r", getHead());
-        wait (1);
-    }
-    */
-    move(500);
-    wait(0.5);
-    turn(1);
-    wait(0.5);
+    
+//    while (1){
+//        pc.printf("Heading: %f\n\r", getHead());
+//        wait (1);
+//    }
     
-    move(500);
-    wait(0.5);
-    turn(1);
-    wait(0.5);
+    coord(0,2);
+    coord(2,2);
+    coord(2,0);
+    coord(0,0);
     
-    move(500);
-    wait(0.5);
-    turn(1);
-    wait(0.5);
-    
-    move(500);
+    speaker.period(1.0/note3);
+    speaker =0.25;
+    wait(1);
+    speaker=0.0;
 }