DERPS
Dependencies: BMP280 BNO055_fusion PowerControl mbed
Fork of STRAIGHT_DRIVE_NO_SEP by
Diff: main.cpp
- Revision:
- 39:ecc9defe3dc0
- Parent:
- 38:16208e003dc9
diff -r 16208e003dc9 -r ecc9defe3dc0 main.cpp --- a/main.cpp Wed Apr 27 04:24:11 2016 +0000 +++ b/main.cpp Wed Apr 27 17:51:17 2016 +0000 @@ -5,6 +5,10 @@ #define WATCHDOG_TIME 10 //#define PC_MODE 1 +#define IMPELLER_OFF 1000 +#define IMPELLER_SPEED 1285 +#define IMPELLER_STEPS 5 +#define IMPELLER_STEP ((IMPELLER_SPEED-IMPELLER_OFF)/IMPELLER_STEPS) #if defined (PC_MODE) Serial pc(USBTX, USBRX); @@ -35,17 +39,9 @@ bool xGood=false; bool yGood=false; bool angleGood=false; -bool flip=false; -int xState=X_INCREASE; int angleTarget=355; -int yTarget=ROB_SIZE/2; int pressure; - -//bool flag=false; -//int target=20; -//int angle_error=2; -//bool xGood=false; -//int angleTarget=0; +int cmd=-1; int main() { @@ -54,8 +50,6 @@ //Initialize to 0 suction.pulsewidth_us(1000); wait(2); - /*suction.pulsewidth_us(1000); - wait(2);*/ //Watchdog Reset Indicator if ((LPC_WDT->WDMOD>>2)&1) led4=1; @@ -72,35 +66,32 @@ //pres.initialize(); //Attach Periodic Wireless Printing #if not defined(PC_MODE) + pc.attach(&callback); t.attach(&send,1); #endif led1=0; led2=0; led3=0; led4=0; - suction.pulsewidth_us(1000); - wait(0.5); - suction.pulsewidth_us(1050); - wait(0.5); - suction.pulsewidth_us(1100); - wait(0.5); - suction.pulsewidth_us(1150); - wait(0.5); - suction.pulsewidth_us(1200); - wait(0.5); - suction.pulsewidth_us(1250); - wait(0.5); + for(int i=0; i<IMPELLER_STEPS; i++) { + suction.pulsewidth_us((i*IMPELLER_STEP)+IMPELLER_OFF); + wait(0.5); + } while(1) { suction.pulsewidth_us(1285); - //uncomment this part if you want the robot to just drive down the window with no separtor - if (xya.y>FRAME_H*0.65) { - while(1) - { + //Finish + if (xya.y>FRAME_H*0.75) { + while(1) { + uint8_t i=0; suction.pulsewidth_us(1285); motion.mStop(); + led1= (i & (1<<0))>>0; + led2= (i & (1<<1))>>1; + led3= (i & (1<<2))>>2; + led4= (i & (1<<3))>>3; + i++; safe.kick(); } - //continue; } loc.get_xy(&xya); xGood=motion.setXPos(xTarget,xya.x,2,0); @@ -110,20 +101,35 @@ xTarget=(xTarget==FRAME_W)?0:FRAME_W; angleTarget=(angleTarget==5)?-5:5; } - //motion.setYBias(0,xya.y,2,angleTarget); - //loc.get_xy(&xya);5 #if defined(PC_MODE) - pc.printf("X: %3d\tY: %3d\tP: %3d\txGood: %d\tAngleGood: %d\tState: %d \n",xya.y,yTarget,xya.a,xGood,angleGood,xState); + pc.printf("X: %3d\tY: %3d\tP: %3d\txGood: %d\tAngleGood: %d\tState: %d \n",xya.y,yTarget,xya.a,xGood,angleGood,xState); #endif //Feed the dog safe.kick(); } } +void callback() +{ + __disable_irq(); + while(pc.readable()==1) + cmd=pc.getc(); + switch(cmd) { + case 2: + + break; + default: + break; + } + __enable_irq(); +} + void send() { - //Print over serial port to WiFi MCU - pc.printf("%c%c%c%c\n",(char)xya.x,(char)xya.y,xya.a/10,xya.a%10); + if(pc.readable()==0) { + //Print over serial port to WiFi MCU + pc.printf("%c%c%c%c\n",(char)xya.x,(char)xya.y,xya.a/10,xya.a%10); + } } void BrownOut()