I want go to sea
Dependencies: arrc_mbed Servo air
main.cpp@1:d3985e5af20f, 2022-03-11 (annotated)
- Committer:
- darkumatar
- Date:
- Fri Mar 11 00:45:18 2022 +0000
- Revision:
- 1:d3985e5af20f
- Parent:
- 0:d7ecfe1f9840
- Child:
- 2:fe26901d9d49
ppap
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
darkumatar | 0:d7ecfe1f9840 | 1 | #include "mbed.h" |
darkumatar | 0:d7ecfe1f9840 | 2 | #include "scrp_slave.hpp" |
darkumatar | 0:d7ecfe1f9840 | 3 | #include "air.hpp" |
darkumatar | 0:d7ecfe1f9840 | 4 | #include "Servo.h" |
darkumatar | 0:d7ecfe1f9840 | 5 | ScrpSlave slave(PC_12,PD_2,PH_1,SERIAL_TX,SERIAL_RX,0x0807ffff); |
darkumatar | 0:d7ecfe1f9840 | 6 | Serial pc(SERIAL_TX,SERIAL_RX); |
darkumatar | 0:d7ecfe1f9840 | 7 | DigitalIn limitA(PB_6,PullUp); |
darkumatar | 0:d7ecfe1f9840 | 8 | DigitalIn limitB(PB_12,PullUp); |
darkumatar | 0:d7ecfe1f9840 | 9 | Air catch_no(PA_9); |
darkumatar | 1:d3985e5af20f | 10 | //Air catch_no2(PA_7); |
darkumatar | 1:d3985e5af20f | 11 | //Air catch_no3(PA_6); |
darkumatar | 0:d7ecfe1f9840 | 12 | |
darkumatar | 0:d7ecfe1f9840 | 13 | DigitalIn limitC(PB_4,PullUp); |
darkumatar | 0:d7ecfe1f9840 | 14 | Air hassya(PA_8); |
darkumatar | 1:d3985e5af20f | 15 | Servo myservo(PA_11); |
darkumatar | 0:d7ecfe1f9840 | 16 | Timer timer; |
darkumatar | 1:d3985e5af20f | 17 | int tim=-6000; |
darkumatar | 1:d3985e5af20f | 18 | bool limit_switch = true; |
darkumatar | 1:d3985e5af20f | 19 | int date=0; |
darkumatar | 1:d3985e5af20f | 20 | int air_date=0; |
darkumatar | 0:d7ecfe1f9840 | 21 | |
darkumatar | 0:d7ecfe1f9840 | 22 | |
darkumatar | 0:d7ecfe1f9840 | 23 | void driveMotorS2(double pwm){ |
darkumatar | 0:d7ecfe1f9840 | 24 | PwmOut up_pin_A(PB_13); |
darkumatar | 0:d7ecfe1f9840 | 25 | PwmOut up_pin_B(PB_14); |
darkumatar | 0:d7ecfe1f9840 | 26 | up_pin_A.period_us(2048); |
darkumatar | 0:d7ecfe1f9840 | 27 | up_pin_B.period_us(2048); |
darkumatar | 1:d3985e5af20f | 28 | if (pwm==0) { |
darkumatar | 0:d7ecfe1f9840 | 29 | up_pin_A = 0; |
darkumatar | 0:d7ecfe1f9840 | 30 | up_pin_B = 0; |
darkumatar | 0:d7ecfe1f9840 | 31 | } else if (0 < pwm) { |
darkumatar | 0:d7ecfe1f9840 | 32 | up_pin_A = pwm; |
darkumatar | 0:d7ecfe1f9840 | 33 | up_pin_B = 0; |
darkumatar | 0:d7ecfe1f9840 | 34 | } else { |
darkumatar | 0:d7ecfe1f9840 | 35 | up_pin_A = 0; |
darkumatar | 0:d7ecfe1f9840 | 36 | up_pin_B= -pwm; |
darkumatar | 0:d7ecfe1f9840 | 37 | } |
darkumatar | 0:d7ecfe1f9840 | 38 | } |
darkumatar | 0:d7ecfe1f9840 | 39 | // ↓サーボ用 |
darkumatar | 0:d7ecfe1f9840 | 40 | void Servo(bool data){ |
darkumatar | 0:d7ecfe1f9840 | 41 | if(data==0){ |
darkumatar | 0:d7ecfe1f9840 | 42 | myservo = 0; |
darkumatar | 0:d7ecfe1f9840 | 43 | }else{ |
darkumatar | 0:d7ecfe1f9840 | 44 | myservo = 0.2; |
darkumatar | 0:d7ecfe1f9840 | 45 | } |
darkumatar | 0:d7ecfe1f9840 | 46 | } |
darkumatar | 0:d7ecfe1f9840 | 47 | //↓発射用 |
darkumatar | 1:d3985e5af20f | 48 | int a=0; |
darkumatar | 0:d7ecfe1f9840 | 49 | bool Hassya(int rx_data,int &tx_data){ |
darkumatar | 1:d3985e5af20f | 50 | if(rx_data==1 /*&& a==1*/){ |
darkumatar | 0:d7ecfe1f9840 | 51 | tim=timer.read_ms(); |
darkumatar | 1:d3985e5af20f | 52 | a=0; |
darkumatar | 0:d7ecfe1f9840 | 53 | } |
darkumatar | 1:d3985e5af20f | 54 | if(a==1 && !limitA==0){ |
darkumatar | 1:d3985e5af20f | 55 | hassya.move(0); |
darkumatar | 1:d3985e5af20f | 56 | driveMotorS2(0); |
darkumatar | 1:d3985e5af20f | 57 | Servo(0); |
darkumatar | 1:d3985e5af20f | 58 | a=1; |
darkumatar | 1:d3985e5af20f | 59 | }else if(timer.read_ms()-tim<5000){ |
darkumatar | 1:d3985e5af20f | 60 | hassya.move(0); |
darkumatar | 1:d3985e5af20f | 61 | driveMotorS2(0); |
darkumatar | 1:d3985e5af20f | 62 | Servo(1); |
darkumatar | 1:d3985e5af20f | 63 | }else if(timer.read_ms()-tim<6000){ |
darkumatar | 0:d7ecfe1f9840 | 64 | hassya.move(1); |
darkumatar | 0:d7ecfe1f9840 | 65 | driveMotorS2(0); |
darkumatar | 1:d3985e5af20f | 66 | Servo(0); |
darkumatar | 1:d3985e5af20f | 67 | }else if(timer.read_ms()-tim>6000 && !limitA==1){ |
darkumatar | 1:d3985e5af20f | 68 | hassya.move(1); |
darkumatar | 1:d3985e5af20f | 69 | driveMotorS2(-0.06); |
darkumatar | 1:d3985e5af20f | 70 | Servo(0); |
darkumatar | 1:d3985e5af20f | 71 | }else { |
darkumatar | 0:d7ecfe1f9840 | 72 | hassya.move(0); |
darkumatar | 0:d7ecfe1f9840 | 73 | driveMotorS2(0); |
darkumatar | 0:d7ecfe1f9840 | 74 | Servo(0); |
darkumatar | 0:d7ecfe1f9840 | 75 | } |
darkumatar | 0:d7ecfe1f9840 | 76 | return true; |
darkumatar | 0:d7ecfe1f9840 | 77 | } |
darkumatar | 0:d7ecfe1f9840 | 78 | |
darkumatar | 0:d7ecfe1f9840 | 79 | //↓モーター |
darkumatar | 0:d7ecfe1f9840 | 80 | void driveMotorS(double pwm){ |
darkumatar | 1:d3985e5af20f | 81 | PwmOut up_pin_A1(PA_1); |
darkumatar | 1:d3985e5af20f | 82 | PwmOut up_pin_B1(PA_0); |
darkumatar | 0:d7ecfe1f9840 | 83 | up_pin_A1.period_us(2048); |
darkumatar | 0:d7ecfe1f9840 | 84 | up_pin_B1.period_us(2048); |
darkumatar | 1:d3985e5af20f | 85 | if (pwm==0) { |
darkumatar | 0:d7ecfe1f9840 | 86 | up_pin_A1 = 0; |
darkumatar | 0:d7ecfe1f9840 | 87 | up_pin_B1 = 0; |
darkumatar | 0:d7ecfe1f9840 | 88 | } else if (0 < pwm) { |
darkumatar | 0:d7ecfe1f9840 | 89 | up_pin_A1 = pwm; |
darkumatar | 0:d7ecfe1f9840 | 90 | up_pin_B1 = 0; |
darkumatar | 0:d7ecfe1f9840 | 91 | } else { |
darkumatar | 0:d7ecfe1f9840 | 92 | up_pin_A1 = 0; |
darkumatar | 0:d7ecfe1f9840 | 93 | up_pin_B1= -pwm; |
darkumatar | 0:d7ecfe1f9840 | 94 | } |
darkumatar | 0:d7ecfe1f9840 | 95 | } |
darkumatar | 0:d7ecfe1f9840 | 96 | //↓昇降 |
darkumatar | 0:d7ecfe1f9840 | 97 | //速さはテキトー |
darkumatar | 0:d7ecfe1f9840 | 98 | bool Up(int rx_data,int &tx_data){ |
darkumatar | 1:d3985e5af20f | 99 | |
darkumatar | 1:d3985e5af20f | 100 | if(rx_data==1){ |
darkumatar | 1:d3985e5af20f | 101 | date=1; |
darkumatar | 1:d3985e5af20f | 102 | } |
darkumatar | 1:d3985e5af20f | 103 | if(rx_data==-1){ |
darkumatar | 1:d3985e5af20f | 104 | date=-1 ; |
darkumatar | 1:d3985e5af20f | 105 | } |
darkumatar | 1:d3985e5af20f | 106 | if(date==1 && !limitA){ |
darkumatar | 1:d3985e5af20f | 107 | driveMotorS(0.6); |
darkumatar | 1:d3985e5af20f | 108 | }else if(date==-1 && !limitB){ |
darkumatar | 1:d3985e5af20f | 109 | driveMotorS(-0.6); |
darkumatar | 0:d7ecfe1f9840 | 110 | }else{ |
darkumatar | 0:d7ecfe1f9840 | 111 | driveMotorS(0); |
darkumatar | 0:d7ecfe1f9840 | 112 | } |
darkumatar | 0:d7ecfe1f9840 | 113 | return true; |
darkumatar | 0:d7ecfe1f9840 | 114 | } |
darkumatar | 0:d7ecfe1f9840 | 115 | //↓回収用 |
darkumatar | 0:d7ecfe1f9840 | 116 | bool CATCH_NO(int rx_data,int &tx_data){ |
darkumatar | 1:d3985e5af20f | 117 | if(tx_data==1){ |
darkumatar | 1:d3985e5af20f | 118 | air_date+=1; |
darkumatar | 1:d3985e5af20f | 119 | }else if(tx_data==2&&air_date==0){ |
darkumatar | 1:d3985e5af20f | 120 | air_date=1; |
darkumatar | 1:d3985e5af20f | 121 | } |
darkumatar | 1:d3985e5af20f | 122 | catch_no.move(air_date%2); |
darkumatar | 1:d3985e5af20f | 123 | return true; |
darkumatar | 1:d3985e5af20f | 124 | } |
darkumatar | 1:d3985e5af20f | 125 | |
darkumatar | 1:d3985e5af20f | 126 | bool CATCH_NO2(int rx_data,int &tx_data){ |
darkumatar | 1:d3985e5af20f | 127 | //catch_no2.move(rx_data); |
darkumatar | 1:d3985e5af20f | 128 | //catch_no3.move(rx_data); |
darkumatar | 0:d7ecfe1f9840 | 129 | return true; |
darkumatar | 0:d7ecfe1f9840 | 130 | } |
darkumatar | 1:d3985e5af20f | 131 | |
darkumatar | 1:d3985e5af20f | 132 | |
darkumatar | 1:d3985e5af20f | 133 | bool limit2(int id,int ppp){ |
darkumatar | 1:d3985e5af20f | 134 | if(ppp==1){ |
darkumatar | 1:d3985e5af20f | 135 | limit_switch=false; |
darkumatar | 1:d3985e5af20f | 136 | driveMotorS(0); |
darkumatar | 1:d3985e5af20f | 137 | driveMotorS2(0); |
darkumatar | 1:d3985e5af20f | 138 | } |
darkumatar | 1:d3985e5af20f | 139 | return true; |
darkumatar | 1:d3985e5af20f | 140 | } |
darkumatar | 1:d3985e5af20f | 141 | |
darkumatar | 1:d3985e5af20f | 142 | bool limit(int a,int &b){ |
darkumatar | 1:d3985e5af20f | 143 | return limit2(0,a); |
darkumatar | 1:d3985e5af20f | 144 | } |
darkumatar | 1:d3985e5af20f | 145 | |
darkumatar | 1:d3985e5af20f | 146 | |
darkumatar | 0:d7ecfe1f9840 | 147 | |
darkumatar | 0:d7ecfe1f9840 | 148 | int main(){ |
darkumatar | 0:d7ecfe1f9840 | 149 | timer.start(); |
darkumatar | 1:d3985e5af20f | 150 | slave.addCMD(1,limit); |
darkumatar | 1:d3985e5af20f | 151 | slave.addCMD(22,Hassya); |
darkumatar | 0:d7ecfe1f9840 | 152 | slave.addCMD(20,CATCH_NO); |
darkumatar | 0:d7ecfe1f9840 | 153 | slave.addCMD(21,Up); |
darkumatar | 1:d3985e5af20f | 154 | //slave.addCMD(22,CATCH_NO2); |
darkumatar | 1:d3985e5af20f | 155 | while(limit_switch){ |
darkumatar | 1:d3985e5af20f | 156 | |
darkumatar | 1:d3985e5af20f | 157 | |
darkumatar | 1:d3985e5af20f | 158 | } |
darkumatar | 1:d3985e5af20f | 159 | driveMotorS(0); |
darkumatar | 1:d3985e5af20f | 160 | driveMotorS2(0); |
darkumatar | 0:d7ecfe1f9840 | 161 | } |