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@23:455f7da3dd7a, 2016-04-03 (annotated)

Committer:
12104404
Date:
Sun Apr 03 07:08:18 2016 +0000
Revision:
23:455f7da3dd7a
Parent:
20:10a305298e27
Child:
24:fb1f083ebd62
Brown Out and LED calling fixed

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 23:455f7da3dd7a 27 LOCOMOTION motion(p11, p21, p22, p23, p24, p15, p16);
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 19:2dd81b864e14 43 //Start Watchdog
12104404 23:455f7da3dd7a 44 safe.kick(WATCHDOG_TIME);
12104404 23:455f7da3dd7a 45 //Power Down Ethernet and USB and Enable Brown Out Interrupt
12104404 23:455f7da3dd7a 46 safe.init((unsigned)BrownOut);
12104404 19:2dd81b864e14 47 //Serial Baudrate
12104404 0:96d6eb224379 48 pc.baud(9600);
12104404 19:2dd81b864e14 49 //Attach Periodic Wireless Printing
12104404 19:2dd81b864e14 50 #if not defined(PC_MODE)
12104404 8:b36be08c44f8 51 t.attach(&send,1);
12104404 19:2dd81b864e14 52 #endif
12104404 0:96d6eb224379 53 while(1) {
12104404 19:2dd81b864e14 54 //loc.get_xy(&xya);
12104404 19:2dd81b864e14 55 /*if(motion.setAngle(angleTarget,xya.a,angle_error,ANGLE_TURN)) {
12104404 18:f9012e93edb8 56 xGood = motion.setXPos(target,xya.x,2,angleTarget);
12104404 18:f9012e93edb8 57 if(motion.setYPos(130,xya.y,2,angleTarget)) {
12104404 17:2f89826b5679 58 angle_error=2;
12104404 18:f9012e93edb8 59 } else if(xGood) {
12104404 18:f9012e93edb8 60 target=target==20?80:20;
12104404 18:f9012e93edb8 61 angleTarget=angleTarget==0?180:0;
12104404 18:f9012e93edb8 62 angle_error=2;
12104404 18:f9012e93edb8 63 } else
12104404 17:2f89826b5679 64 angle_error=10;
12104404 19:2dd81b864e14 65 }*/
12104404 23:455f7da3dd7a 66
12104404 23:455f7da3dd7a 67
12104404 19:2dd81b864e14 68 #if defined(PC_MODE)
12104404 19:2dd81b864e14 69 pc.printf("X: %3d\tY: %3d\tP: %3d\n",xya.x,xya.y,xya.a);
12104404 19:2dd81b864e14 70 #endif
12104404 19:2dd81b864e14 71 //Feed the dog
12104404 23:455f7da3dd7a 72 safe.kick();
12104404 0:96d6eb224379 73 }
12104404 0:96d6eb224379 74 }
12104404 6:0602a9e8118b 75
12104404 6:0602a9e8118b 76 void send()
12104404 6:0602a9e8118b 77 {
12104404 19:2dd81b864e14 78 //Print over serial port to WiFi MCU
12104404 8:b36be08c44f8 79 pc.printf("%c%c%c%c\n",(char)xya.x,(char)xya.y,xya.a/10,xya.a%10);
12104404 6:0602a9e8118b 80 }
12104404 17:2f89826b5679 81
12104404 19:2dd81b864e14 82 void BrownOut()
12104404 6:0602a9e8118b 83 {
12104404 19:2dd81b864e14 84 //Ensure suction on
12104404 23:455f7da3dd7a 85 motion.eStop();
12104404 19:2dd81b864e14 86 suction.pulsewidth_us(2000);
12104404 23:455f7da3dd7a 87 led1=1;
12104404 23:455f7da3dd7a 88 led2=0;
12104404 23:455f7da3dd7a 89 led3=1;
12104404 23:455f7da3dd7a 90 led4=0;
12104404 19:2dd81b864e14 91 //Power Down Non Essential
12104404 19:2dd81b864e14 92 Peripheral_PowerDown(~(LPC1768_PCONP_PCPWM1 | LPC1768_PCONP_PCGPIO | LPC1768_PCONP_PCTIM0));
12104404 19:2dd81b864e14 93 //Sleep wake by interrupt
12104404 23:455f7da3dd7a 94 //Sleep();
12104404 17:2f89826b5679 95 }