Diff: main.cpp

Revision:

1:7937f28c742d

Parent:

0:417f3396cf67

Child:

2:c37a8b45b96f

--- a/main.cpp	Wed Feb 02 11:27:13 2022 +0000
+++ b/main.cpp	Tue Mar 15 08:44:02 2022 +0000
@@ -4,11 +4,16 @@
 #include "motor.hpp"
 
 //-----Classes-----
-ScrpSlave slave(PC_12, PD_2, PH_1, SERIAL_TX, SERIAL_RX, 4);   //id: 4
-Motor motor(PB_14, PB_13);
+ScrpSlave slave(PA_9, PA_10, PA_12, SERIAL_TX, SERIAL_RX, 4);   //id: 4
+Motor motor1(PA_11, PB_6);//左
+Motor motor2(PA_3, PA_1);//右
+DigitalIn Limitswitch1(PA_0);
+DigitalIn Limitswitch2(PA_4);
+DigitalIn Limitswitch3(PB_0);
+DigitalIn Limitswitch4(PB_1);
 
 //-----Constants-----
-const double pwm = 0.1;
+const double pwm = 0.45;
 
 //-----Sub Routines-----
 bool Up(int RX,int &TX);
@@ -17,13 +22,31 @@
 //-----Variables-----
 int up = 0;
 int down = 0;
+double PWM1 = 0.0;
+double PWM2 = 0.0;
+int limitswitch1 = 1;
+int limitswitch2 = 1;
+int limitswitch3 = 1;
+int limitswitch4 = 1;
 
 //-----Main Routine-----
 int main(){
     slave.addCMD(1, Up);
     slave.addCMD(2, Down);
+    Limitswitch1.mode(PullUp);
+    Limitswitch2.mode(PullUp);
+    Limitswitch3.mode(PullUp);
+    Limitswitch4.mode(PullUp);
     while(1){
-        motor.out(pwm * (up + down)); 
+        limitswitch1 = Limitswitch1.read() ? 0 : 1;
+        limitswitch2 = Limitswitch2.read() ? 0 : 1;
+        limitswitch3 = Limitswitch3.read() ? 0 : 1;
+        limitswitch4 = Limitswitch4.read() ? 0 : 1; 
+        printf("%d %d %d %d %d %d\n", up, down, limitswitch1, limitswitch2, limitswitch3, limitswitch4);
+        PWM1 = pwm * ((up * limitswitch1) - (down * limitswitch3));
+        PWM2 = pwm * ((up * limitswitch2) - (down * limitswitch4));
+        motor1.out(PWM1);
+        motor2.out(PWM2);
     }
 }
 
@@ -33,6 +56,6 @@
 }
 
 bool Down(int RX, int &TX){
-    down = -RX;
+    down = RX;
     return true;
 }
\ No newline at end of file