農作物回収機構プログラム
Dependencies: mbed arrc_mbed Motor
main.cpp@2:c37a8b45b96f, 2022-03-28 (annotated)
- Committer:
- m2130
- Date:
- Mon Mar 28 04:17:20 2022 +0000
- Revision:
- 2:c37a8b45b96f
- Parent:
- 1:7937f28c742d
awawawawawawwawawa
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
m2130 | 0:417f3396cf67 | 1 | //-----Libraries----- |
m2130 | 0:417f3396cf67 | 2 | #include "mbed.h" |
m2130 | 0:417f3396cf67 | 3 | #include "scrp_slave.hpp" |
m2130 | 0:417f3396cf67 | 4 | #include "motor.hpp" |
m2130 | 0:417f3396cf67 | 5 | |
m2130 | 0:417f3396cf67 | 6 | //-----Classes----- |
m2130 | 1:7937f28c742d | 7 | ScrpSlave slave(PA_9, PA_10, PA_12, SERIAL_TX, SERIAL_RX, 4); //id: 4 |
m2130 | 1:7937f28c742d | 8 | Motor motor1(PA_11, PB_6);//左 |
m2130 | 1:7937f28c742d | 9 | Motor motor2(PA_3, PA_1);//右 |
m2130 | 1:7937f28c742d | 10 | DigitalIn Limitswitch1(PA_0); |
m2130 | 1:7937f28c742d | 11 | DigitalIn Limitswitch2(PA_4); |
m2130 | 1:7937f28c742d | 12 | DigitalIn Limitswitch3(PB_0); |
m2130 | 1:7937f28c742d | 13 | DigitalIn Limitswitch4(PB_1); |
m2130 | 0:417f3396cf67 | 14 | |
m2130 | 0:417f3396cf67 | 15 | //-----Constants----- |
m2130 | 2:c37a8b45b96f | 16 | const double pwm = 0.4; |
m2130 | 0:417f3396cf67 | 17 | |
m2130 | 0:417f3396cf67 | 18 | //-----Sub Routines----- |
m2130 | 0:417f3396cf67 | 19 | bool Up(int RX,int &TX); |
m2130 | 0:417f3396cf67 | 20 | bool Down(int RX,int &TX); |
m2130 | 0:417f3396cf67 | 21 | |
m2130 | 0:417f3396cf67 | 22 | //-----Variables----- |
m2130 | 0:417f3396cf67 | 23 | int up = 0; |
m2130 | 0:417f3396cf67 | 24 | int down = 0; |
m2130 | 1:7937f28c742d | 25 | double PWM1 = 0.0; |
m2130 | 1:7937f28c742d | 26 | double PWM2 = 0.0; |
m2130 | 1:7937f28c742d | 27 | int limitswitch1 = 1; |
m2130 | 1:7937f28c742d | 28 | int limitswitch2 = 1; |
m2130 | 1:7937f28c742d | 29 | int limitswitch3 = 1; |
m2130 | 1:7937f28c742d | 30 | int limitswitch4 = 1; |
m2130 | 0:417f3396cf67 | 31 | |
m2130 | 0:417f3396cf67 | 32 | //-----Main Routine----- |
m2130 | 0:417f3396cf67 | 33 | int main(){ |
m2130 | 0:417f3396cf67 | 34 | slave.addCMD(1, Up); |
m2130 | 0:417f3396cf67 | 35 | slave.addCMD(2, Down); |
m2130 | 1:7937f28c742d | 36 | Limitswitch1.mode(PullUp); |
m2130 | 1:7937f28c742d | 37 | Limitswitch2.mode(PullUp); |
m2130 | 1:7937f28c742d | 38 | Limitswitch3.mode(PullUp); |
m2130 | 1:7937f28c742d | 39 | Limitswitch4.mode(PullUp); |
m2130 | 0:417f3396cf67 | 40 | while(1){ |
m2130 | 1:7937f28c742d | 41 | limitswitch1 = Limitswitch1.read() ? 0 : 1; |
m2130 | 1:7937f28c742d | 42 | limitswitch2 = Limitswitch2.read() ? 0 : 1; |
m2130 | 1:7937f28c742d | 43 | limitswitch3 = Limitswitch3.read() ? 0 : 1; |
m2130 | 1:7937f28c742d | 44 | limitswitch4 = Limitswitch4.read() ? 0 : 1; |
m2130 | 1:7937f28c742d | 45 | printf("%d %d %d %d %d %d\n", up, down, limitswitch1, limitswitch2, limitswitch3, limitswitch4); |
m2130 | 1:7937f28c742d | 46 | PWM1 = pwm * ((up * limitswitch1) - (down * limitswitch3)); |
m2130 | 2:c37a8b45b96f | 47 | PWM2 = pwm * ((up * limitswitch2) - (down * limitswitch4)) * 0.8; |
m2130 | 1:7937f28c742d | 48 | motor1.out(PWM1); |
m2130 | 1:7937f28c742d | 49 | motor2.out(PWM2); |
m2130 | 0:417f3396cf67 | 50 | } |
m2130 | 0:417f3396cf67 | 51 | } |
m2130 | 0:417f3396cf67 | 52 | |
m2130 | 0:417f3396cf67 | 53 | bool Up(int RX, int &TX){ |
m2130 | 2:c37a8b45b96f | 54 | if(RX == 1)up = RX; |
m2130 | 0:417f3396cf67 | 55 | return true; |
m2130 | 0:417f3396cf67 | 56 | } |
m2130 | 0:417f3396cf67 | 57 | |
m2130 | 0:417f3396cf67 | 58 | bool Down(int RX, int &TX){ |
m2130 | 1:7937f28c742d | 59 | down = RX; |
m2130 | 2:c37a8b45b96f | 60 | if(RX == 1)up = 0; |
m2130 | 0:417f3396cf67 | 61 | return true; |
m2130 | 0:417f3396cf67 | 62 | } |