Diff: main.cpp

Revision:

30:116cd143fdf7

Parent:

28:65daccc10f31

Child:

31:69caabc10879

--- a/main.cpp	Wed Apr 13 07:48:49 2016 +0000
+++ b/main.cpp	Tue Apr 19 02:04:10 2016 +0000
@@ -1,6 +1,7 @@
 #include "LOCALIZE.h"
 #include "LOCOMOTION.h"
 #include "SAFETY.h"
+#include "BMP280.h"
 
 #define WATCHDOG_TIME   10
 //#define PC_MODE         1
@@ -22,6 +23,7 @@
 PwmOut suction(p26);
 I2C i2c2(p28, p27);
 I2C i2c1(p9, p10);
+BMP280 pres(i2c2);
 LOCALIZE loc(i2c1, i2c2, p29, p8, p7, p6, p5, led1, led2, led3, led4);
 LOCALIZE_xya xya;
 LOCOMOTION motion(p11, p23, p24, p21, p22, p16, p15, led1, led2, led3, led4);
@@ -29,14 +31,15 @@
 
 void BrownOut();
 
-int xTarget=20;
+int xTarget=120;
 int angle_error=5;
 bool xGood=false;
 bool yGood=false;
 bool angleGood=false;
 int xState=X_INCREASE;
-int angleTarget=0;
-int yTarget=30;
+int angleTarget=5;
+int yTarget=80;
+int pressure;
 
 //bool flag=false;
 //int target=20;
@@ -66,6 +69,7 @@
     pc.baud(9600);
     //Initialize Locomotion
     loc.init();
+    //pres.initialize();
     //Attach Periodic Wireless Printing
 #if not defined(PC_MODE)
     t.attach(&send,1);
@@ -74,12 +78,31 @@
     led2=0;
     led3=0;
     led4=0;
+    suction.pulsewidth_us(1200);
+    wait(0.5);
+    suction.pulsewidth_us(1300);
+    wait(0.5);
+    suction.pulsewidth_us(1400);
+    wait(0.5);
+    suction.pulsewidth_us(1500);
+    wait(0.5);
+    suction.pulsewidth_us(1600);
+    wait(0.5);
     while(1) {
-        suction.pulsewidth_us(1600);
+        suction.pulsewidth_us(1750);
         //loc.get_angle(&xya);
         loc.get_xy(&xya);
-        //motion.setAngle(90,xya.a,angle_error,ANGLE_TURN);
-        //motion.moveF(xya.a);
+        //pressure=(int)read[0];//(int)pres.getTemperature();
+        //if(pressure==88)
+        //    led1=1;
+        //else
+        //    led1=0;
+        /*if(motion.setAngle(5,xya.a,angle_error,ANGLE_TURN))
+        {
+            motion.setXPos(xTarget,xya.x,2,angleTarget);
+            motion.setYBias(yTarget,xya.y,2,angleTarget);
+        }*/
+            //motion.moveF(xya.a);
         /*if(abs(xya.a-180)<90)
             motion.moveB();
         else
@@ -114,7 +137,7 @@
 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);
+    pc.printf("%c%c%c%c\n",(char)xya.x,(char)xya.y,pressure/10,pressure%10);
 }
 
 void BrownOut()