Diff: main.cpp

Revision:

23:455f7da3dd7a

Parent:

20:10a305298e27

Child:

24:fb1f083ebd62

--- a/main.cpp	Sun Apr 03 06:15:52 2016 +0000
+++ b/main.cpp	Sun Apr 03 07:08:18 2016 +0000
@@ -1,12 +1,9 @@
-#include "PowerControl/PowerControl.h"
-#include "PowerControl/EthernetPowerControl.h"
 #include "LOCALIZE.h"
 #include "LOCOMOTION.h"
-#include "WATCHDOG.h"
+#include "SAFETY.h"
 
 #define WATCHDOG_TIME   10
 //#define PC_MODE         1
-#define USR_POWERDOWN   (0x104)
 
 #if defined (PC_MODE)
 Serial pc(USBTX, USBRX);
@@ -16,19 +13,18 @@
 void send();
 #endif
 
-int semihost_powerdown()
-{
-    uint32_t arg;
-    return __semihost(USR_POWERDOWN, &arg);
-}
+DigitalOut led1(LED1);
+DigitalOut led2(LED2);
+DigitalOut led3(LED3);
+DigitalOut led4(LED4);
 
-Watchdog wdt;
+SAFETY safe;
 PwmOut suction(p26);
 I2C i2c1(p28, p27);
 I2C i2c2(p9, p10);
-LOCALIZE loc(i2c1, i2c2, p29, p8, p7, p6, p5);
+LOCALIZE loc(i2c1, i2c2, p29, p8, p7, p6, p5, led1, led2, led3, led4);
 LOCALIZE_xya xya;
-LOCOMOTION motion(p21, p22, p23, p24, p15, p16);
+LOCOMOTION motion(p11, p21, p22, p23, p24, p15, p16);
 
 void BrownOut();
 
@@ -45,15 +41,9 @@
     //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);
+    safe.kick(WATCHDOG_TIME);
+    //Power Down Ethernet and USB and Enable Brown Out Interrupt
+    safe.init((unsigned)BrownOut);
     //Serial Baudrate
     pc.baud(9600);
     //Attach Periodic Wireless Printing
@@ -73,11 +63,13 @@
             } else
                 angle_error=10;
         }*/
+
+        
 #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();
+        safe.kick();
     }
 }
 
@@ -90,10 +82,14 @@
 void BrownOut()
 {
     //Ensure suction on
-    motion.stopMotors();
+    motion.eStop();
     suction.pulsewidth_us(2000);
+    led1=1;
+    led2=0;
+    led3=1;
+    led4=0;
     //Power Down Non Essential
     Peripheral_PowerDown(~(LPC1768_PCONP_PCPWM1 | LPC1768_PCONP_PCGPIO | LPC1768_PCONP_PCTIM0));
     //Sleep wake by interrupt
-    Sleep();
+    //Sleep();
 }
\ No newline at end of file