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@30:116cd143fdf7, 2016-04-19 (annotated)

Committer:
12104404
Date:
Tue Apr 19 02:04:10 2016 +0000
Revision:
30:116cd143fdf7
Parent:
28:65daccc10f31
Child:
31:69caabc10879
explode

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 30:116cd143fdf7 4 #include "BMP280.h"
12104404 0:96d6eb224379 5
12104404 19:2dd81b864e14 6 #define WATCHDOG_TIME 10
12104404 27:fb1298d35bd1 7 //#define PC_MODE 1
12104404 6:0602a9e8118b 8
12104404 19:2dd81b864e14 9 #if defined (PC_MODE)
12104404 19:2dd81b864e14 10 Serial pc(USBTX, USBRX);
12104404 19:2dd81b864e14 11 #else
12104404 8:b36be08c44f8 12 Serial pc(p13, p14);
12104404 19:2dd81b864e14 13 Ticker t;
12104404 19:2dd81b864e14 14 void send();
12104404 19:2dd81b864e14 15 #endif
12104404 19:2dd81b864e14 16
12104404 23:455f7da3dd7a 17 DigitalOut led1(LED1);
12104404 23:455f7da3dd7a 18 DigitalOut led2(LED2);
12104404 23:455f7da3dd7a 19 DigitalOut led3(LED3);
12104404 23:455f7da3dd7a 20 DigitalOut led4(LED4);
12104404 6:0602a9e8118b 21
12104404 23:455f7da3dd7a 22 SAFETY safe;
12104404 19:2dd81b864e14 23 PwmOut suction(p26);
12104404 27:fb1298d35bd1 24 I2C i2c2(p28, p27);
12104404 27:fb1298d35bd1 25 I2C i2c1(p9, p10);
12104404 30:116cd143fdf7 26 BMP280 pres(i2c2);
12104404 23:455f7da3dd7a 27 LOCALIZE loc(i2c1, i2c2, p29, p8, p7, p6, p5, led1, led2, led3, led4);
12104404 0:96d6eb224379 28 LOCALIZE_xya xya;
12104404 27:fb1298d35bd1 29 LOCOMOTION motion(p11, p23, p24, p21, p22, p16, p15, led1, led2, led3, led4);
12104404 27:fb1298d35bd1 30 //LOCOMOTION motion(p11, p21, p22, p23, p24, p15, p16, led1, led2, led3, led4);
12104404 6:0602a9e8118b 31
12104404 20:10a305298e27 32 void BrownOut();
12104404 6:0602a9e8118b 33
12104404 30:116cd143fdf7 34 int xTarget=120;
12104404 27:fb1298d35bd1 35 int angle_error=5;
12104404 25:f3bf8351bbd4 36 bool xGood=false;
12104404 25:f3bf8351bbd4 37 bool yGood=false;
12104404 25:f3bf8351bbd4 38 bool angleGood=false;
12104404 25:f3bf8351bbd4 39 int xState=X_INCREASE;
12104404 30:116cd143fdf7 40 int angleTarget=5;
12104404 30:116cd143fdf7 41 int yTarget=80;
12104404 30:116cd143fdf7 42 int pressure;
12104404 25:f3bf8351bbd4 43
12104404 20:10a305298e27 44 //bool flag=false;
12104404 20:10a305298e27 45 //int target=20;
12104404 20:10a305298e27 46 //int angle_error=2;
12104404 20:10a305298e27 47 //bool xGood=false;
12104404 20:10a305298e27 48 //int angleTarget=0;
12104404 6:0602a9e8118b 49
12104404 0:96d6eb224379 50 int main()
12104404 0:96d6eb224379 51 {
12104404 19:2dd81b864e14 52 //Set ESC Period
12104404 19:2dd81b864e14 53 suction.period_ms(2);
12104404 19:2dd81b864e14 54 //Initialize to 0
12104404 19:2dd81b864e14 55 suction.pulsewidth_us(1000);
12104404 27:fb1298d35bd1 56 wait(2);
12104404 27:fb1298d35bd1 57 /*suction.pulsewidth_us(1000);
12104404 27:fb1298d35bd1 58 wait(2);*/
12104404 24:fb1f083ebd62 59 //Watchdog Reset Indicator
12104404 24:fb1f083ebd62 60 if ((LPC_WDT->WDMOD>>2)&1)
12104404 24:fb1f083ebd62 61 led4=1;
12104404 24:fb1f083ebd62 62 else
12104404 24:fb1f083ebd62 63 led3=1;
12104404 19:2dd81b864e14 64 //Start Watchdog
12104404 23:455f7da3dd7a 65 safe.kick(WATCHDOG_TIME);
12104404 23:455f7da3dd7a 66 //Power Down Ethernet and USB and Enable Brown Out Interrupt
12104404 23:455f7da3dd7a 67 safe.init((unsigned)BrownOut);
12104404 19:2dd81b864e14 68 //Serial Baudrate
12104404 0:96d6eb224379 69 pc.baud(9600);
12104404 26:0ea6a550a99d 70 //Initialize Locomotion
12104404 26:0ea6a550a99d 71 loc.init();
12104404 30:116cd143fdf7 72 //pres.initialize();
12104404 19:2dd81b864e14 73 //Attach Periodic Wireless Printing
12104404 19:2dd81b864e14 74 #if not defined(PC_MODE)
12104404 8:b36be08c44f8 75 t.attach(&send,1);
12104404 19:2dd81b864e14 76 #endif
12104404 24:fb1f083ebd62 77 led1=0;
12104404 24:fb1f083ebd62 78 led2=0;
12104404 24:fb1f083ebd62 79 led3=0;
12104404 24:fb1f083ebd62 80 led4=0;
12104404 30:116cd143fdf7 81 suction.pulsewidth_us(1200);
12104404 30:116cd143fdf7 82 wait(0.5);
12104404 30:116cd143fdf7 83 suction.pulsewidth_us(1300);
12104404 30:116cd143fdf7 84 wait(0.5);
12104404 30:116cd143fdf7 85 suction.pulsewidth_us(1400);
12104404 30:116cd143fdf7 86 wait(0.5);
12104404 30:116cd143fdf7 87 suction.pulsewidth_us(1500);
12104404 30:116cd143fdf7 88 wait(0.5);
12104404 30:116cd143fdf7 89 suction.pulsewidth_us(1600);
12104404 30:116cd143fdf7 90 wait(0.5);
12104404 0:96d6eb224379 91 while(1) {
12104404 30:116cd143fdf7 92 suction.pulsewidth_us(1750);
12104404 28:65daccc10f31 93 //loc.get_angle(&xya);
12104404 28:65daccc10f31 94 loc.get_xy(&xya);
12104404 30:116cd143fdf7 95 //pressure=(int)read[0];//(int)pres.getTemperature();
12104404 30:116cd143fdf7 96 //if(pressure==88)
12104404 30:116cd143fdf7 97 // led1=1;
12104404 30:116cd143fdf7 98 //else
12104404 30:116cd143fdf7 99 // led1=0;
12104404 30:116cd143fdf7 100 /*if(motion.setAngle(5,xya.a,angle_error,ANGLE_TURN))
12104404 30:116cd143fdf7 101 {
12104404 30:116cd143fdf7 102 motion.setXPos(xTarget,xya.x,2,angleTarget);
12104404 30:116cd143fdf7 103 motion.setYBias(yTarget,xya.y,2,angleTarget);
12104404 30:116cd143fdf7 104 }*/
12104404 30:116cd143fdf7 105 //motion.moveF(xya.a);
12104404 27:fb1298d35bd1 106 /*if(abs(xya.a-180)<90)
12104404 27:fb1298d35bd1 107 motion.moveB();
12104404 27:fb1298d35bd1 108 else
12104404 27:fb1298d35bd1 109 motion.moveB();*/
12104404 27:fb1298d35bd1 110 //loc.get_xy(&xya);
12104404 27:fb1298d35bd1 111
12104404 27:fb1298d35bd1 112 /*if (yTarget>120) {
12104404 26:0ea6a550a99d 113 motion.mStop();
12104404 26:0ea6a550a99d 114 safe.kick();
12104404 26:0ea6a550a99d 115 continue;
12104404 26:0ea6a550a99d 116 }
12104404 26:0ea6a550a99d 117
12104404 25:f3bf8351bbd4 118 loc.get_xy(&xya);
12104404 25:f3bf8351bbd4 119 motion.check_xya(&xGood,&yGood,&angleGood,xTarget,angleTarget,yTarget,xya,3,3,angle_error);
12104404 26:0ea6a550a99d 120
12104404 26:0ea6a550a99d 121 motion.setXstate(&xState,&xTarget,xGood,angleGood,&angleTarget,&angle_error);
12104404 25:f3bf8351bbd4 122 motion.setAngleTol(&angle_error,yGood,xGood);
12104404 25:f3bf8351bbd4 123 motion.setYgoal(xGood,angleGood,yGood,&yTarget);
12104404 25:f3bf8351bbd4 124 if(motion.setAngle(angleTarget,xya.a,angle_error,ANGLE_TURN)) {
12104404 25:f3bf8351bbd4 125 motion.setXPos(xTarget,xya.x,2,angleTarget);
12104404 25:f3bf8351bbd4 126 motion.setYBias(yTarget,xya.y,2,angleTarget);
12104404 25:f3bf8351bbd4 127
12104404 27:fb1298d35bd1 128 }*/
12104404 19:2dd81b864e14 129 #if defined(PC_MODE)
12104404 26:0ea6a550a99d 130 pc.printf("X: %3d\tY: %3d\tP: %3d\txGood: %d\tAngleGood: %d\tState: %d \n",xya.y,yTarget,xya.a,xGood,angleGood,xState);
12104404 19:2dd81b864e14 131 #endif
12104404 19:2dd81b864e14 132 //Feed the dog
12104404 23:455f7da3dd7a 133 safe.kick();
12104404 0:96d6eb224379 134 }
12104404 0:96d6eb224379 135 }
12104404 6:0602a9e8118b 136
12104404 6:0602a9e8118b 137 void send()
12104404 6:0602a9e8118b 138 {
12104404 19:2dd81b864e14 139 //Print over serial port to WiFi MCU
12104404 30:116cd143fdf7 140 pc.printf("%c%c%c%c\n",(char)xya.x,(char)xya.y,pressure/10,pressure%10);
12104404 6:0602a9e8118b 141 }
12104404 17:2f89826b5679 142
12104404 19:2dd81b864e14 143 void BrownOut()
12104404 6:0602a9e8118b 144 {
12104404 24:fb1f083ebd62 145 //Stop Motors and Driver
12104404 23:455f7da3dd7a 146 motion.eStop();
12104404 24:fb1f083ebd62 147 //Ensure suction on
12104404 19:2dd81b864e14 148 suction.pulsewidth_us(2000);
12104404 24:fb1f083ebd62 149 //Light Up Error Light Pattern
12104404 23:455f7da3dd7a 150 led1=1;
12104404 23:455f7da3dd7a 151 led2=0;
12104404 23:455f7da3dd7a 152 led3=1;
12104404 23:455f7da3dd7a 153 led4=0;
12104404 19:2dd81b864e14 154 //Power Down Non Essential
12104404 24:fb1f083ebd62 155 Peripheral_PowerDown(~(LPC1768_PCONP_PCPWM1 | LPC1768_PCONP_PCGPIO));
12104404 19:2dd81b864e14 156 //Sleep wake by interrupt
12104404 23:455f7da3dd7a 157 //Sleep();
12104404 17:2f89826b5679 158 }