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@27:fb1298d35bd1, 2016-04-13 (annotated)

Committer:
12104404
Date:
Wed Apr 13 03:14:43 2016 +0000
Revision:
27:fb1298d35bd1
Parent:
26:0ea6a550a99d
Child:
28:65daccc10f31
gravity compensation;

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 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 27:fb1298d35bd1 79 loc.get_angle(&xya);
12104404 27:fb1298d35bd1 80 motion.setAngle(90,xya.a,angle_error,ANGLE_TURN);
12104404 27:fb1298d35bd1 81 //motion.moveF(xya.a);
12104404 27:fb1298d35bd1 82 /*if(abs(xya.a-180)<90)
12104404 27:fb1298d35bd1 83 motion.moveB();
12104404 27:fb1298d35bd1 84 else
12104404 27:fb1298d35bd1 85 motion.moveB();*/
12104404 27:fb1298d35bd1 86 //loc.get_xy(&xya);
12104404 27:fb1298d35bd1 87
12104404 27:fb1298d35bd1 88 /*if (yTarget>120) {
12104404 26:0ea6a550a99d 89 motion.mStop();
12104404 26:0ea6a550a99d 90 safe.kick();
12104404 26:0ea6a550a99d 91 continue;
12104404 26:0ea6a550a99d 92 }
12104404 26:0ea6a550a99d 93
12104404 25:f3bf8351bbd4 94 loc.get_xy(&xya);
12104404 25:f3bf8351bbd4 95 motion.check_xya(&xGood,&yGood,&angleGood,xTarget,angleTarget,yTarget,xya,3,3,angle_error);
12104404 26:0ea6a550a99d 96
12104404 26:0ea6a550a99d 97 motion.setXstate(&xState,&xTarget,xGood,angleGood,&angleTarget,&angle_error);
12104404 25:f3bf8351bbd4 98 motion.setAngleTol(&angle_error,yGood,xGood);
12104404 25:f3bf8351bbd4 99 motion.setYgoal(xGood,angleGood,yGood,&yTarget);
12104404 25:f3bf8351bbd4 100 if(motion.setAngle(angleTarget,xya.a,angle_error,ANGLE_TURN)) {
12104404 25:f3bf8351bbd4 101 motion.setXPos(xTarget,xya.x,2,angleTarget);
12104404 25:f3bf8351bbd4 102 motion.setYBias(yTarget,xya.y,2,angleTarget);
12104404 25:f3bf8351bbd4 103
12104404 27:fb1298d35bd1 104 }*/
12104404 19:2dd81b864e14 105 #if defined(PC_MODE)
12104404 26:0ea6a550a99d 106 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 107 #endif
12104404 19:2dd81b864e14 108 //Feed the dog
12104404 23:455f7da3dd7a 109 safe.kick();
12104404 0:96d6eb224379 110 }
12104404 0:96d6eb224379 111 }
12104404 6:0602a9e8118b 112
12104404 6:0602a9e8118b 113 void send()
12104404 6:0602a9e8118b 114 {
12104404 19:2dd81b864e14 115 //Print over serial port to WiFi MCU
12104404 8:b36be08c44f8 116 pc.printf("%c%c%c%c\n",(char)xya.x,(char)xya.y,xya.a/10,xya.a%10);
12104404 6:0602a9e8118b 117 }
12104404 17:2f89826b5679 118
12104404 19:2dd81b864e14 119 void BrownOut()
12104404 6:0602a9e8118b 120 {
12104404 24:fb1f083ebd62 121 //Stop Motors and Driver
12104404 23:455f7da3dd7a 122 motion.eStop();
12104404 24:fb1f083ebd62 123 //Ensure suction on
12104404 19:2dd81b864e14 124 suction.pulsewidth_us(2000);
12104404 24:fb1f083ebd62 125 //Light Up Error Light Pattern
12104404 23:455f7da3dd7a 126 led1=1;
12104404 23:455f7da3dd7a 127 led2=0;
12104404 23:455f7da3dd7a 128 led3=1;
12104404 23:455f7da3dd7a 129 led4=0;
12104404 19:2dd81b864e14 130 //Power Down Non Essential
12104404 24:fb1f083ebd62 131 Peripheral_PowerDown(~(LPC1768_PCONP_PCPWM1 | LPC1768_PCONP_PCGPIO));
12104404 19:2dd81b864e14 132 //Sleep wake by interrupt
12104404 23:455f7da3dd7a 133 //Sleep();
12104404 17:2f89826b5679 134 }