Diff: main.cpp

Revision:

2:74c543a0a671

Parent:

1:107a7d8f4c54

Child:

3:d2c733b52600

--- a/main.cpp	Sun Aug 02 01:12:13 2015 +0000
+++ b/main.cpp	Thu Aug 06 14:24:04 2015 +0000
@@ -5,49 +5,56 @@
 #include "Motor.h"
 #include "HMC6352.h"
 BusOut air(p15,p16);
-Serial conn(p29,p28);
+Serial conn(p28,p27);
+Serial pc(USBTX,USBRX);
 BusOut mypaul(p17,p18);
+BusIn sw(p24,p25);
 Motor od(p21,p19,p20);
 Motor ot(p22,p12,p11);
-QEI sensort(p5,p6,NC,720);
+QEI sensort(p29,p30,NC,624);
 HMC6352 sensord(p9,p10);
 char read;
-void serial_thread(void const *argument)
+int main()
 {
+    //sensord.setOpMode(HMC6352_CONTINUOUS,1,20);
+    double direct = 0,tilt = 0,cdirect = 0,adirect = 0,tdirect = 0,ttilt =0,itilt=0;
+    //tdirect = sensord.sample()/10.0;
+    sw.mode(PullUp);
     while(1) {
         if(conn.readable()) {
             read = conn.getc();
         }
-    }
-}
-int main()
-{
-    sensord.setOpMode(HMC6352_CONTINUOUS,1,20);
-    Thread thread(serial_thread);
-    double direct,tilt,cdirect,adirect,atilt,tdirect,ttilt;
-    adirect = sensord.sample()/10.0;
-    sensort.reset();
-    while(1) {
         air = read%4;
         mypaul = (read>>2)%4;
-        if((read>>4)%2) {
-            tdirect++;
-        }
-        if((read>>5)%2) {
-            tdirect--;
+        if((read>>4)%2 || 0/*sw == 2*/) {
+            //tdirect++;
+            od.speed(0.3);
+        } else if((read>>5)%2 ||0 /*sw == 1*/) {
+            //tdirect--;
+            od.speed(-0.3);
+        } else {
+            od.speed(0);
         }
-        if((read>>6)%2) {
-            ttilt++;
+        if((read>>6)%2|| sw == 2 ) {
+            //ttilt+=pi/180.0*0.01;
+            ot.speed(0.3);
+        } else if((read>>7)%2 || sw == 1) {
+            //ttilt-=pi/180.0*0.01;
+            ot.speed(-0.3);
+        } else {
+            ot.speed(0);
         }
-        if((read>>7)%2) {
-            ttilt--;
+        if(tdirect >= 180 ||tdirect <= -180) {
+            //tdirect *= -1;
         }
-        cdirect = sensord.sample()/10.0;
-        direct = cdirect+540-adirect;
+        //cdirect = sensord.sample()/10.0;
+        direct = cdirect+540-tdirect;
         direct = int(direct+0.5)%360;
         direct -= 180;
-        tilt = sensort.getPulses()/360.0*2/25.0*pi;
-        od.speed(tdirect-direct);
-        ot.speed(ttilt-tilt);
+        //tilt = sensort.getPulses()/360.0/25.0*pi;
+        pc.printf("%d\n\r",read);
+        //od.speed(cdirect);
+        //ot.speed((ttilt-tilt)*1.0+itilt*0.01);
+        itilt += ttilt-tilt;
     }
 }