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@24:fb1f083ebd62, 2016-04-03 (annotated)

Committer:
12104404
Date:
Sun Apr 03 07:25:07 2016 +0000
Revision:
24:fb1f083ebd62
Parent:
23:455f7da3dd7a
Child:
25:f3bf8351bbd4
Added LED Error Indicators

Who changed what in which revision?

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