Antoine Laurens / Mbed 2 deprecated STRAIGHT_DRIVE_NO_SEP

drive down

Dependencies:   BMP280 BNO055_fusion PowerControl mbed

Fork of TEAM_G_FLOW_RIDA by Edwin Cho

main.cpp@19:2dd81b864e14, 2016-04-03 (annotated)

Committer:
12104404
Date:
Sun Apr 03 05:48:37 2016 +0000
Revision:
19:2dd81b864e14
Parent:
18:f9012e93edb8
Child:
20:10a305298e27
Cleaning up code;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
12104404 19:2dd81b864e14 1 #include "PowerControl/PowerControl.h"
12104404 19:2dd81b864e14 2 #include "PowerControl/EthernetPowerControl.h"
12104404 0:96d6eb224379 3 #include "LOCALIZE.h"
12104404 6:0602a9e8118b 4 #include "LOCOMOTION.h"
12104404 6:0602a9e8118b 5 #include "WATCHDOG.h"
12104404 0:96d6eb224379 6
12104404 19:2dd81b864e14 7 #define WATCHDOG_TIME 10
12104404 19:2dd81b864e14 8 //#define PC_MODE 1
12104404 19:2dd81b864e14 9 #define USR_POWERDOWN (0x104)
12104404 6:0602a9e8118b 10
12104404 19:2dd81b864e14 11 #if defined (PC_MODE)
12104404 19:2dd81b864e14 12 Serial pc(USBTX, USBRX);
12104404 19:2dd81b864e14 13 #else
12104404 8:b36be08c44f8 14 Serial pc(p13, p14);
12104404 19:2dd81b864e14 15 Ticker t;
12104404 19:2dd81b864e14 16 void send();
12104404 19:2dd81b864e14 17 #endif
12104404 19:2dd81b864e14 18
12104404 19:2dd81b864e14 19 int semihost_powerdown()
12104404 19:2dd81b864e14 20 {
12104404 19:2dd81b864e14 21 uint32_t arg;
12104404 19:2dd81b864e14 22 return __semihost(USR_POWERDOWN, &arg);
12104404 19:2dd81b864e14 23 }
12104404 6:0602a9e8118b 24
12104404 6:0602a9e8118b 25 Watchdog wdt;
12104404 19:2dd81b864e14 26 PwmOut suction(p26);
12104404 0:96d6eb224379 27 I2C i2c1(p28, p27);
12104404 0:96d6eb224379 28 I2C i2c2(p9, p10);
12104404 19:2dd81b864e14 29 LOCALIZE loc(i2c1, i2c2, p29, p8, p7, p6, p5);
12104404 0:96d6eb224379 30 LOCALIZE_xya xya;
12104404 15:7729da55873a 31 LOCOMOTION motion(p21, p22, p23, p24, p15, p16);
12104404 6:0602a9e8118b 32
12104404 6:0602a9e8118b 33 bool flag=false;
12104404 17:2f89826b5679 34 int target=20;
12104404 17:2f89826b5679 35 int angle_error=2;
12104404 17:2f89826b5679 36 bool xGood=false;
12104404 18:f9012e93edb8 37 int angleTarget=0;
12104404 6:0602a9e8118b 38
12104404 19:2dd81b864e14 39 void BrownOut();
12104404 6:0602a9e8118b 40
12104404 0:96d6eb224379 41 int main()
12104404 0:96d6eb224379 42 {
12104404 19:2dd81b864e14 43 //Set ESC Period
12104404 19:2dd81b864e14 44 suction.period_ms(2);
12104404 19:2dd81b864e14 45 //Initialize to 0
12104404 19:2dd81b864e14 46 suction.pulsewidth_us(1000);
12104404 19:2dd81b864e14 47 //Start Watchdog
12104404 19:2dd81b864e14 48 wdt.kick(WATCHDOG_TIME);
12104404 19:2dd81b864e14 49 //Power Down Ethernet
12104404 19:2dd81b864e14 50 PHY_PowerDown();
12104404 19:2dd81b864e14 51 //Power Down USB Chip
12104404 19:2dd81b864e14 52 semihost_powerdown();
12104404 19:2dd81b864e14 53 //Setup Brown Out Interrupt
12104404 19:2dd81b864e14 54 NVIC_SetVector(BOD_IRQn, (unsigned)BrownOut);
12104404 19:2dd81b864e14 55 //Enable Brown Out Interrupt
12104404 19:2dd81b864e14 56 NVIC_EnableIRQ(BOD_IRQn);
12104404 19:2dd81b864e14 57 //Serial Baudrate
12104404 0:96d6eb224379 58 pc.baud(9600);
12104404 19:2dd81b864e14 59 //Attach Periodic Wireless Printing
12104404 19:2dd81b864e14 60 #if not defined(PC_MODE)
12104404 8:b36be08c44f8 61 t.attach(&send,1);
12104404 19:2dd81b864e14 62 #endif
12104404 0:96d6eb224379 63 while(1) {
12104404 19:2dd81b864e14 64 //loc.get_xy(&xya);
12104404 19:2dd81b864e14 65 /*if(motion.setAngle(angleTarget,xya.a,angle_error,ANGLE_TURN)) {
12104404 18:f9012e93edb8 66 xGood = motion.setXPos(target,xya.x,2,angleTarget);
12104404 18:f9012e93edb8 67 if(motion.setYPos(130,xya.y,2,angleTarget)) {
12104404 17:2f89826b5679 68 angle_error=2;
12104404 18:f9012e93edb8 69 } else if(xGood) {
12104404 18:f9012e93edb8 70 target=target==20?80:20;
12104404 18:f9012e93edb8 71 angleTarget=angleTarget==0?180:0;
12104404 18:f9012e93edb8 72 angle_error=2;
12104404 18:f9012e93edb8 73 } else
12104404 17:2f89826b5679 74 angle_error=10;
12104404 19:2dd81b864e14 75 }*/
12104404 19:2dd81b864e14 76 #if defined(PC_MODE)
12104404 19:2dd81b864e14 77 pc.printf("X: %3d\tY: %3d\tP: %3d\n",xya.x,xya.y,xya.a);
12104404 19:2dd81b864e14 78 #endif
12104404 19:2dd81b864e14 79 //Feed the dog
12104404 6:0602a9e8118b 80 wdt.kick();
12104404 0:96d6eb224379 81 }
12104404 0:96d6eb224379 82 }
12104404 6:0602a9e8118b 83
12104404 6:0602a9e8118b 84 void send()
12104404 6:0602a9e8118b 85 {
12104404 19:2dd81b864e14 86 //Print over serial port to WiFi MCU
12104404 8:b36be08c44f8 87 pc.printf("%c%c%c%c\n",(char)xya.x,(char)xya.y,xya.a/10,xya.a%10);
12104404 6:0602a9e8118b 88 }
12104404 17:2f89826b5679 89
12104404 19:2dd81b864e14 90 void BrownOut()
12104404 6:0602a9e8118b 91 {
12104404 19:2dd81b864e14 92 //Ensure suction on
12104404 19:2dd81b864e14 93 motion.stopMotors();
12104404 19:2dd81b864e14 94 suction.pulsewidth_us(2000);
12104404 19:2dd81b864e14 95 //Power Down Non Essential
12104404 19:2dd81b864e14 96 Peripheral_PowerDown(~(LPC1768_PCONP_PCPWM1 | LPC1768_PCONP_PCGPIO | LPC1768_PCONP_PCTIM0));
12104404 19:2dd81b864e14 97 //Sleep wake by interrupt
12104404 19:2dd81b864e14 98 Sleep();
12104404 17:2f89826b5679 99 }