![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
test0417
Diff: main.cpp
- Revision:
- 7:46bb04ae559c
- Parent:
- 5:221ce6ba655c
- Child:
- 8:31d4dd20cfc0
--- a/main.cpp Tue Mar 26 01:16:55 2019 +0000 +++ b/main.cpp Wed Mar 27 05:27:59 2019 +0000 @@ -8,6 +8,9 @@ //DigitalOut led_red(LED_RED); DigitalOut led1(LED1); + +DigitalOut LIFT_UP_PIN(D4); +DigitalOut LIFT_DOWN_PIN(D3); //InterruptIn sw2(B1); //I2C i2c( PTB1, PTB0); I2C i2c( D14, D15); @@ -38,6 +41,8 @@ void Driving_Right(int rightOutPut); void Driving_Left(int leftOutPut); +void liftup(int stage); + void controlMotorSpeed(float goalLinearVel, float goalAngularVel); int main() @@ -55,6 +60,8 @@ Driving_Right(motorOutPut); wait_ms(100); Driving_Left(motorOutPut); + + liftup(0); while (true) { @@ -90,7 +97,28 @@ else if (lin_vel2 > -LIMIT_X_MIN_VELOCITY && lin_vel2 < 0) lin_vel2 = -LIMIT_X_MIN_VELOCITY; } +void liftup(int stage) +{ + switch (stage) { + case 0: //stop + LIFT_UP_PIN=0; + LIFT_DOWN_PIN=0; + break; + case 1: //up + LIFT_UP_PIN=1; + LIFT_DOWN_PIN=0; + + break; + case 2: //down + LIFT_UP_PIN=0; + LIFT_DOWN_PIN=1; + + break; + + } + +} void Driving_Right(int rightOutPut) { Rmotobuffer[0]= 0x00; @@ -115,5 +143,6 @@ i2c.write(Lmotobuffer[1]); i2c.write(Lmotobuffer[2]); i2c.stop(); + }