added
Dependencies: BNO055_fusion PowerControl mbed
Fork of TEAM_G_FLOW_RIDA by
main.cpp@20:10a305298e27, 2016-04-03 (annotated)
- Committer:
- 12104404
- Date:
- Sun Apr 03 06:08:08 2016 +0000
- Revision:
- 20:10a305298e27
- Parent:
- 19:2dd81b864e14
- Child:
- 23:455f7da3dd7a
MEKATRONICS TEAM G
Who changed what in which revision?
User | Revision | Line number | New 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 | 20:10a305298e27 | 33 | void BrownOut(); |
12104404 | 6:0602a9e8118b | 34 | |
12104404 | 20:10a305298e27 | 35 | //bool flag=false; |
12104404 | 20:10a305298e27 | 36 | //int target=20; |
12104404 | 20:10a305298e27 | 37 | //int angle_error=2; |
12104404 | 20:10a305298e27 | 38 | //bool xGood=false; |
12104404 | 20:10a305298e27 | 39 | //int angleTarget=0; |
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 | } |