drive down
Dependencies: BMP280 BNO055_fusion PowerControl mbed
Fork of TEAM_G_FLOW_RIDA by
main.cpp@28:65daccc10f31, 2016-04-13 (annotated)
- Committer:
- 12104404
- Date:
- Wed Apr 13 07:19:18 2016 +0000
- Revision:
- 28:65daccc10f31
- Parent:
- 27:fb1298d35bd1
- Child:
- 30:116cd143fdf7
ultra working p4
Who changed what in which revision?
User | Revision | Line number | New 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 | 27:fb1298d35bd1 | 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 | 27:fb1298d35bd1 | 23 | I2C i2c2(p28, p27); |
12104404 | 27:fb1298d35bd1 | 24 | I2C i2c1(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 | 27:fb1298d35bd1 | 27 | LOCOMOTION motion(p11, p23, p24, p21, p22, p16, p15, led1, led2, led3, led4); |
12104404 | 27:fb1298d35bd1 | 28 | //LOCOMOTION motion(p11, p21, p22, p23, p24, p15, p16, led1, led2, led3, led4); |
12104404 | 6:0602a9e8118b | 29 | |
12104404 | 20:10a305298e27 | 30 | void BrownOut(); |
12104404 | 6:0602a9e8118b | 31 | |
12104404 | 25:f3bf8351bbd4 | 32 | int xTarget=20; |
12104404 | 27:fb1298d35bd1 | 33 | int angle_error=5; |
12104404 | 25:f3bf8351bbd4 | 34 | bool xGood=false; |
12104404 | 25:f3bf8351bbd4 | 35 | bool yGood=false; |
12104404 | 25:f3bf8351bbd4 | 36 | bool angleGood=false; |
12104404 | 25:f3bf8351bbd4 | 37 | int xState=X_INCREASE; |
12104404 | 25:f3bf8351bbd4 | 38 | int angleTarget=0; |
12104404 | 26:0ea6a550a99d | 39 | int yTarget=30; |
12104404 | 25:f3bf8351bbd4 | 40 | |
12104404 | 20:10a305298e27 | 41 | //bool flag=false; |
12104404 | 20:10a305298e27 | 42 | //int target=20; |
12104404 | 20:10a305298e27 | 43 | //int angle_error=2; |
12104404 | 20:10a305298e27 | 44 | //bool xGood=false; |
12104404 | 20:10a305298e27 | 45 | //int angleTarget=0; |
12104404 | 6:0602a9e8118b | 46 | |
12104404 | 0:96d6eb224379 | 47 | int main() |
12104404 | 0:96d6eb224379 | 48 | { |
12104404 | 19:2dd81b864e14 | 49 | //Set ESC Period |
12104404 | 19:2dd81b864e14 | 50 | suction.period_ms(2); |
12104404 | 19:2dd81b864e14 | 51 | //Initialize to 0 |
12104404 | 19:2dd81b864e14 | 52 | suction.pulsewidth_us(1000); |
12104404 | 27:fb1298d35bd1 | 53 | wait(2); |
12104404 | 27:fb1298d35bd1 | 54 | /*suction.pulsewidth_us(1000); |
12104404 | 27:fb1298d35bd1 | 55 | wait(2);*/ |
12104404 | 24:fb1f083ebd62 | 56 | //Watchdog Reset Indicator |
12104404 | 24:fb1f083ebd62 | 57 | if ((LPC_WDT->WDMOD>>2)&1) |
12104404 | 24:fb1f083ebd62 | 58 | led4=1; |
12104404 | 24:fb1f083ebd62 | 59 | else |
12104404 | 24:fb1f083ebd62 | 60 | led3=1; |
12104404 | 19:2dd81b864e14 | 61 | //Start Watchdog |
12104404 | 23:455f7da3dd7a | 62 | safe.kick(WATCHDOG_TIME); |
12104404 | 23:455f7da3dd7a | 63 | //Power Down Ethernet and USB and Enable Brown Out Interrupt |
12104404 | 23:455f7da3dd7a | 64 | safe.init((unsigned)BrownOut); |
12104404 | 19:2dd81b864e14 | 65 | //Serial Baudrate |
12104404 | 0:96d6eb224379 | 66 | pc.baud(9600); |
12104404 | 26:0ea6a550a99d | 67 | //Initialize Locomotion |
12104404 | 26:0ea6a550a99d | 68 | loc.init(); |
12104404 | 19:2dd81b864e14 | 69 | //Attach Periodic Wireless Printing |
12104404 | 19:2dd81b864e14 | 70 | #if not defined(PC_MODE) |
12104404 | 8:b36be08c44f8 | 71 | t.attach(&send,1); |
12104404 | 19:2dd81b864e14 | 72 | #endif |
12104404 | 24:fb1f083ebd62 | 73 | led1=0; |
12104404 | 24:fb1f083ebd62 | 74 | led2=0; |
12104404 | 24:fb1f083ebd62 | 75 | led3=0; |
12104404 | 24:fb1f083ebd62 | 76 | led4=0; |
12104404 | 0:96d6eb224379 | 77 | while(1) { |
12104404 | 27:fb1298d35bd1 | 78 | suction.pulsewidth_us(1600); |
12104404 | 28:65daccc10f31 | 79 | //loc.get_angle(&xya); |
12104404 | 28:65daccc10f31 | 80 | loc.get_xy(&xya); |
12104404 | 28:65daccc10f31 | 81 | //motion.setAngle(90,xya.a,angle_error,ANGLE_TURN); |
12104404 | 27:fb1298d35bd1 | 82 | //motion.moveF(xya.a); |
12104404 | 27:fb1298d35bd1 | 83 | /*if(abs(xya.a-180)<90) |
12104404 | 27:fb1298d35bd1 | 84 | motion.moveB(); |
12104404 | 27:fb1298d35bd1 | 85 | else |
12104404 | 27:fb1298d35bd1 | 86 | motion.moveB();*/ |
12104404 | 27:fb1298d35bd1 | 87 | //loc.get_xy(&xya); |
12104404 | 27:fb1298d35bd1 | 88 | |
12104404 | 27:fb1298d35bd1 | 89 | /*if (yTarget>120) { |
12104404 | 26:0ea6a550a99d | 90 | motion.mStop(); |
12104404 | 26:0ea6a550a99d | 91 | safe.kick(); |
12104404 | 26:0ea6a550a99d | 92 | continue; |
12104404 | 26:0ea6a550a99d | 93 | } |
12104404 | 26:0ea6a550a99d | 94 | |
12104404 | 25:f3bf8351bbd4 | 95 | loc.get_xy(&xya); |
12104404 | 25:f3bf8351bbd4 | 96 | motion.check_xya(&xGood,&yGood,&angleGood,xTarget,angleTarget,yTarget,xya,3,3,angle_error); |
12104404 | 26:0ea6a550a99d | 97 | |
12104404 | 26:0ea6a550a99d | 98 | motion.setXstate(&xState,&xTarget,xGood,angleGood,&angleTarget,&angle_error); |
12104404 | 25:f3bf8351bbd4 | 99 | motion.setAngleTol(&angle_error,yGood,xGood); |
12104404 | 25:f3bf8351bbd4 | 100 | motion.setYgoal(xGood,angleGood,yGood,&yTarget); |
12104404 | 25:f3bf8351bbd4 | 101 | if(motion.setAngle(angleTarget,xya.a,angle_error,ANGLE_TURN)) { |
12104404 | 25:f3bf8351bbd4 | 102 | motion.setXPos(xTarget,xya.x,2,angleTarget); |
12104404 | 25:f3bf8351bbd4 | 103 | motion.setYBias(yTarget,xya.y,2,angleTarget); |
12104404 | 25:f3bf8351bbd4 | 104 | |
12104404 | 27:fb1298d35bd1 | 105 | }*/ |
12104404 | 19:2dd81b864e14 | 106 | #if defined(PC_MODE) |
12104404 | 26:0ea6a550a99d | 107 | 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 | 108 | #endif |
12104404 | 19:2dd81b864e14 | 109 | //Feed the dog |
12104404 | 23:455f7da3dd7a | 110 | safe.kick(); |
12104404 | 0:96d6eb224379 | 111 | } |
12104404 | 0:96d6eb224379 | 112 | } |
12104404 | 6:0602a9e8118b | 113 | |
12104404 | 6:0602a9e8118b | 114 | void send() |
12104404 | 6:0602a9e8118b | 115 | { |
12104404 | 19:2dd81b864e14 | 116 | //Print over serial port to WiFi MCU |
12104404 | 8:b36be08c44f8 | 117 | pc.printf("%c%c%c%c\n",(char)xya.x,(char)xya.y,xya.a/10,xya.a%10); |
12104404 | 6:0602a9e8118b | 118 | } |
12104404 | 17:2f89826b5679 | 119 | |
12104404 | 19:2dd81b864e14 | 120 | void BrownOut() |
12104404 | 6:0602a9e8118b | 121 | { |
12104404 | 24:fb1f083ebd62 | 122 | //Stop Motors and Driver |
12104404 | 23:455f7da3dd7a | 123 | motion.eStop(); |
12104404 | 24:fb1f083ebd62 | 124 | //Ensure suction on |
12104404 | 19:2dd81b864e14 | 125 | suction.pulsewidth_us(2000); |
12104404 | 24:fb1f083ebd62 | 126 | //Light Up Error Light Pattern |
12104404 | 23:455f7da3dd7a | 127 | led1=1; |
12104404 | 23:455f7da3dd7a | 128 | led2=0; |
12104404 | 23:455f7da3dd7a | 129 | led3=1; |
12104404 | 23:455f7da3dd7a | 130 | led4=0; |
12104404 | 19:2dd81b864e14 | 131 | //Power Down Non Essential |
12104404 | 24:fb1f083ebd62 | 132 | Peripheral_PowerDown(~(LPC1768_PCONP_PCPWM1 | LPC1768_PCONP_PCGPIO)); |
12104404 | 19:2dd81b864e14 | 133 | //Sleep wake by interrupt |
12104404 | 23:455f7da3dd7a | 134 | //Sleep(); |
12104404 | 17:2f89826b5679 | 135 | } |