Diff: main.cpp

Revision:

6:46bb04ae559c

Parent:

5:221ce6ba655c

--- 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();
+
 }