2022_gorobo_Ateam / Mbed 2 deprecated harvest

農作物回収機構プログラム

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?

UserRevisionLine numberNew 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 }