Diff: main.cpp

Revision:

19:2dd81b864e14

Parent:

18:f9012e93edb8

Child:

20:10a305298e27

--- a/main.cpp	Tue Mar 29 02:12:08 2016 +0000
+++ b/main.cpp	Sun Apr 03 05:48:37 2016 +0000
@@ -1,45 +1,68 @@
+#include "PowerControl/PowerControl.h"
+#include "PowerControl/EthernetPowerControl.h"
 #include "LOCALIZE.h"
 #include "LOCOMOTION.h"
 #include "WATCHDOG.h"
 
-#define SPEED_FB_MIN    0.15
-#define SPEED_FB_MAX    0.50
+#define WATCHDOG_TIME   10
+//#define PC_MODE         1
+#define USR_POWERDOWN   (0x104)
 
+#if defined (PC_MODE)
+Serial pc(USBTX, USBRX);
+#else
 Serial pc(p13, p14);
-//Serial pc(USBTX, USBRX);
+Ticker t;
+void send();
+#endif
+
+int semihost_powerdown()
+{
+    uint32_t arg;
+    return __semihost(USR_POWERDOWN, &arg);
+}
 
 Watchdog wdt;
-
+PwmOut suction(p26);
 I2C i2c1(p28, p27);
 I2C i2c2(p9, p10);
-LOCALIZE loc(i2c1, i2c2, p26, p8, p7, p6, p5);
+LOCALIZE loc(i2c1, i2c2, p29, p8, p7, p6, p5);
 LOCALIZE_xya xya;
 LOCOMOTION motion(p21, p22, p23, p24, p15, p16);
 
-Ticker t;
-Ticker tTarget;
 bool flag=false;
 int target=20;
 int angle_error=2;
 bool xGood=false;
 int angleTarget=0;
 
-void setTarget();
-void send();
-//void setAngle(int angle);
-int wrap(int a);
+void BrownOut();
 
 int main()
 {
-    wdt.kick(5);
+    //Set ESC Period
+    suction.period_ms(2);
+    //Initialize to 0
+    suction.pulsewidth_us(1000);
+    //Start Watchdog
+    wdt.kick(WATCHDOG_TIME);
+    //Power Down Ethernet
+    PHY_PowerDown();
+    //Power Down USB Chip
+    semihost_powerdown();
+    //Setup Brown Out Interrupt
+    NVIC_SetVector(BOD_IRQn, (unsigned)BrownOut);
+    //Enable Brown Out Interrupt
+    NVIC_EnableIRQ(BOD_IRQn);
+    //Serial Baudrate
     pc.baud(9600);
-    //pc.printf("Initialized Localization: %d\n",loc.init());
+    //Attach Periodic Wireless Printing
+#if not defined(PC_MODE)
     t.attach(&send,1);
-    //tTarget.attach(&setTarget,7);
+#endif
     while(1) {
-        //loc.get_angle(&xya);
-        loc.get_xy(&xya);
-        if(motion.setAngle(angleTarget,xya.a,angle_error,ANGLE_TURN)) {
+        //loc.get_xy(&xya);
+        /*if(motion.setAngle(angleTarget,xya.a,angle_error,ANGLE_TURN)) {
             xGood = motion.setXPos(target,xya.x,2,angleTarget);
             if(motion.setYPos(130,xya.y,2,angleTarget)) {
                 angle_error=2;
@@ -49,18 +72,28 @@
                 angle_error=2;
             } else
                 angle_error=10;
-        }
-        //pc.printf("X: %3d\tY: %3d\tP: %3d\n",xya.x,xya.y,xya.a);
+        }*/
+#if defined(PC_MODE)
+        pc.printf("X: %3d\tY: %3d\tP: %3d\n",xya.x,xya.y,xya.a);
+#endif
+        //Feed the dog
         wdt.kick();
     }
 }
 
 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);
 }
 
-void setTarget()
+void BrownOut()
 {
-    target=target==20?80:20;
+    //Ensure suction on
+    motion.stopMotors();
+    suction.pulsewidth_us(2000);
+    //Power Down Non Essential
+    Peripheral_PowerDown(~(LPC1768_PCONP_PCPWM1 | LPC1768_PCONP_PCGPIO | LPC1768_PCONP_PCTIM0));
+    //Sleep wake by interrupt
+    Sleep();
 }
\ No newline at end of file