Diff: main.cpp

Revision:

39:ecc9defe3dc0

Parent:

38:16208e003dc9

--- a/main.cpp	Wed Apr 27 04:24:11 2016 +0000
+++ b/main.cpp	Wed Apr 27 17:51:17 2016 +0000
@@ -5,6 +5,10 @@
 
 #define WATCHDOG_TIME   10
 //#define PC_MODE         1
+#define IMPELLER_OFF    1000
+#define IMPELLER_SPEED  1285
+#define IMPELLER_STEPS  5
+#define IMPELLER_STEP   ((IMPELLER_SPEED-IMPELLER_OFF)/IMPELLER_STEPS)
 
 #if defined (PC_MODE)
 Serial pc(USBTX, USBRX);
@@ -35,17 +39,9 @@
 bool xGood=false;
 bool yGood=false;
 bool angleGood=false;
-bool flip=false;
-int xState=X_INCREASE;
 int angleTarget=355;
-int yTarget=ROB_SIZE/2;
 int pressure;
-
-//bool flag=false;
-//int target=20;
-//int angle_error=2;
-//bool xGood=false;
-//int angleTarget=0;
+int cmd=-1;
 
 int main()
 {
@@ -54,8 +50,6 @@
     //Initialize to 0
     suction.pulsewidth_us(1000);
     wait(2);
-    /*suction.pulsewidth_us(1000);
-    wait(2);*/
     //Watchdog Reset Indicator
     if ((LPC_WDT->WDMOD>>2)&1)
         led4=1;
@@ -72,35 +66,32 @@
     //pres.initialize();
     //Attach Periodic Wireless Printing
 #if not defined(PC_MODE)
+    pc.attach(&callback);
     t.attach(&send,1);
 #endif
     led1=0;
     led2=0;
     led3=0;
     led4=0;
-    suction.pulsewidth_us(1000);
-    wait(0.5);
-    suction.pulsewidth_us(1050);
-    wait(0.5);
-    suction.pulsewidth_us(1100);
-    wait(0.5);
-    suction.pulsewidth_us(1150);
-    wait(0.5);
-    suction.pulsewidth_us(1200);
-    wait(0.5);
-    suction.pulsewidth_us(1250);
-    wait(0.5);
+    for(int i=0; i<IMPELLER_STEPS; i++) {
+        suction.pulsewidth_us((i*IMPELLER_STEP)+IMPELLER_OFF);
+        wait(0.5);
+    }
     while(1) {
         suction.pulsewidth_us(1285);
-        //uncomment this part if you want the robot to just drive down the window with no separtor
-        if (xya.y>FRAME_H*0.65) {
-            while(1)
-            {
+        //Finish
+        if (xya.y>FRAME_H*0.75) {
+            while(1) {
+                uint8_t i=0;
                 suction.pulsewidth_us(1285);
                 motion.mStop();
+                led1= (i & (1<<0))>>0;
+                led2= (i & (1<<1))>>1;
+                led3= (i & (1<<2))>>2;
+                led4= (i & (1<<3))>>3;
+                i++;
                 safe.kick();
             }
-            //continue;
         }
         loc.get_xy(&xya);
         xGood=motion.setXPos(xTarget,xya.x,2,0);
@@ -110,20 +101,35 @@
             xTarget=(xTarget==FRAME_W)?0:FRAME_W;
             angleTarget=(angleTarget==5)?-5:5;
         }
-            //motion.setYBias(0,xya.y,2,angleTarget);
-            //loc.get_xy(&xya);5
 #if defined(PC_MODE)
-            pc.printf("X: %3d\tY: %3d\tP: %3d\txGood: %d\tAngleGood: %d\tState: %d \n",xya.y,yTarget,xya.a,xGood,angleGood,xState);
+        pc.printf("X: %3d\tY: %3d\tP: %3d\txGood: %d\tAngleGood: %d\tState: %d \n",xya.y,yTarget,xya.a,xGood,angleGood,xState);
 #endif
         //Feed the dog
         safe.kick();
     }
 }
 
+void callback()
+{
+    __disable_irq();
+    while(pc.readable()==1)
+        cmd=pc.getc();
+    switch(cmd) {
+        case 2:
+            
+        break;
+        default:
+        break;        
+    }
+    __enable_irq();
+}
+
 void send()
 {
-    //Print over serial port to WiFi MCU
-    pc.printf("%c%c%c%c\n",(char)xya.x,(char)xya.y,xya.a/10,xya.a%10);
+    if(pc.readable()==0) {
+        //Print over serial port to WiFi MCU
+        pc.printf("%c%c%c%c\n",(char)xya.x,(char)xya.y,xya.a/10,xya.a%10);
+    }
 }
 
 void BrownOut()