Edwin Cho / Mbed 2 deprecated FLOW_DERP

DERPS

Dependencies:   BMP280 BNO055_fusion PowerControl mbed

Fork of STRAIGHT_DRIVE_NO_SEP by Antoine Laurens

main.cpp@39:ecc9defe3dc0, 2016-04-27 (annotated)

Committer:
12104404
Date:
Wed Apr 27 17:51:17 2016 +0000
Revision:
39:ecc9defe3dc0
Parent:
38:16208e003dc9 
derps;

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