農作物回収機構プログラム
Dependencies: mbed arrc_mbed Motor
Diff: main.cpp
- Revision:
- 1:7937f28c742d
- Parent:
- 0:417f3396cf67
- Child:
- 2:c37a8b45b96f
diff -r 417f3396cf67 -r 7937f28c742d main.cpp --- 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