I want go to sea
Dependencies: arrc_mbed Servo air
main.cpp@5:1506bf89f48e, 2022-04-08 (annotated)
- Committer:
- darkumatar
- Date:
- Fri Apr 08 09:29:59 2022 +0000
- Revision:
- 5:1506bf89f48e
- Parent:
- 4:7e2aa008f84b
a;
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 | 3:ccd2dd7d3814 | 8 | DigitalIn limitB(PB_4,PullUp); |
darkumatar | 3:ccd2dd7d3814 | 9 | |
darkumatar | 3:ccd2dd7d3814 | 10 | DigitalIn limitA2(PC_2,PullUp); |
darkumatar | 3:ccd2dd7d3814 | 11 | DigitalIn limitB2(PC_3,PullUp); |
darkumatar | 3:ccd2dd7d3814 | 12 | |
darkumatar | 0:d7ecfe1f9840 | 13 | Air catch_no(PA_9); |
darkumatar | 3:ccd2dd7d3814 | 14 | Air catch_no2(PB_8); |
darkumatar | 1:d3985e5af20f | 15 | //Air catch_no3(PA_6); |
darkumatar | 0:d7ecfe1f9840 | 16 | |
darkumatar | 3:ccd2dd7d3814 | 17 | DigitalIn limitC(PB_5,PullUp); |
darkumatar | 0:d7ecfe1f9840 | 18 | Air hassya(PA_8); |
darkumatar | 1:d3985e5af20f | 19 | Servo myservo(PA_11); |
darkumatar | 0:d7ecfe1f9840 | 20 | Timer timer; |
darkumatar | 3:ccd2dd7d3814 | 21 | int tim=-2000; |
darkumatar | 1:d3985e5af20f | 22 | bool limit_switch = true; |
darkumatar | 1:d3985e5af20f | 23 | int date=0; |
darkumatar | 3:ccd2dd7d3814 | 24 | int date2=0; |
darkumatar | 1:d3985e5af20f | 25 | int air_date=0; |
darkumatar | 2:fe26901d9d49 | 26 | int air_date2=0; |
darkumatar | 3:ccd2dd7d3814 | 27 | int limit_air=0; |
darkumatar | 2:fe26901d9d49 | 28 | int a=0; |
darkumatar | 0:d7ecfe1f9840 | 29 | |
darkumatar | 0:d7ecfe1f9840 | 30 | |
darkumatar | 0:d7ecfe1f9840 | 31 | void driveMotorS2(double pwm){ |
darkumatar | 0:d7ecfe1f9840 | 32 | PwmOut up_pin_A(PB_13); |
darkumatar | 0:d7ecfe1f9840 | 33 | PwmOut up_pin_B(PB_14); |
darkumatar | 0:d7ecfe1f9840 | 34 | up_pin_A.period_us(2048); |
darkumatar | 0:d7ecfe1f9840 | 35 | up_pin_B.period_us(2048); |
darkumatar | 1:d3985e5af20f | 36 | if (pwm==0) { |
darkumatar | 0:d7ecfe1f9840 | 37 | up_pin_A = 0; |
darkumatar | 0:d7ecfe1f9840 | 38 | up_pin_B = 0; |
darkumatar | 0:d7ecfe1f9840 | 39 | } else if (0 < pwm) { |
darkumatar | 0:d7ecfe1f9840 | 40 | up_pin_A = pwm; |
darkumatar | 0:d7ecfe1f9840 | 41 | up_pin_B = 0; |
darkumatar | 0:d7ecfe1f9840 | 42 | } else { |
darkumatar | 0:d7ecfe1f9840 | 43 | up_pin_A = 0; |
darkumatar | 0:d7ecfe1f9840 | 44 | up_pin_B= -pwm; |
darkumatar | 0:d7ecfe1f9840 | 45 | } |
darkumatar | 0:d7ecfe1f9840 | 46 | } |
darkumatar | 0:d7ecfe1f9840 | 47 | // ↓サーボ用 |
darkumatar | 0:d7ecfe1f9840 | 48 | void Servo(bool data){ |
darkumatar | 0:d7ecfe1f9840 | 49 | if(data==0){ |
darkumatar | 0:d7ecfe1f9840 | 50 | myservo = 0; |
darkumatar | 0:d7ecfe1f9840 | 51 | }else{ |
darkumatar | 2:fe26901d9d49 | 52 | myservo = 0.5; |
darkumatar | 0:d7ecfe1f9840 | 53 | } |
darkumatar | 0:d7ecfe1f9840 | 54 | } |
darkumatar | 0:d7ecfe1f9840 | 55 | //↓発射用 |
darkumatar | 0:d7ecfe1f9840 | 56 | bool Hassya(int rx_data,int &tx_data){ |
darkumatar | 2:fe26901d9d49 | 57 | if(rx_data==1 && a==1){ |
darkumatar | 0:d7ecfe1f9840 | 58 | tim=timer.read_ms(); |
darkumatar | 1:d3985e5af20f | 59 | a=0; |
darkumatar | 0:d7ecfe1f9840 | 60 | } |
darkumatar | 5:1506bf89f48e | 61 | if(timer.read_ms()-tim<500){ |
darkumatar | 1:d3985e5af20f | 62 | hassya.move(0); |
darkumatar | 1:d3985e5af20f | 63 | driveMotorS2(0); |
darkumatar | 1:d3985e5af20f | 64 | Servo(1); |
darkumatar | 5:1506bf89f48e | 65 | }else if(timer.read_ms()-tim<1000){ |
darkumatar | 0:d7ecfe1f9840 | 66 | hassya.move(1); |
darkumatar | 0:d7ecfe1f9840 | 67 | driveMotorS2(0); |
darkumatar | 1:d3985e5af20f | 68 | Servo(0); |
darkumatar | 5:1506bf89f48e | 69 | }else if((timer.read_ms()-tim>1000 && !limitC==0) || a==1){ |
darkumatar | 5:1506bf89f48e | 70 | hassya.move(0); |
darkumatar | 5:1506bf89f48e | 71 | driveMotorS2(0); |
darkumatar | 5:1506bf89f48e | 72 | Servo(0); |
darkumatar | 5:1506bf89f48e | 73 | a=1; |
darkumatar | 5:1506bf89f48e | 74 | }else if(timer.read_ms()-tim>1000 && a==0){ |
darkumatar | 1:d3985e5af20f | 75 | hassya.move(1); |
darkumatar | 5:1506bf89f48e | 76 | driveMotorS2(-0.1); |
darkumatar | 1:d3985e5af20f | 77 | Servo(0); |
darkumatar | 2:fe26901d9d49 | 78 | }else{ |
darkumatar | 0:d7ecfe1f9840 | 79 | hassya.move(0); |
darkumatar | 0:d7ecfe1f9840 | 80 | driveMotorS2(0); |
darkumatar | 0:d7ecfe1f9840 | 81 | Servo(0); |
darkumatar | 0:d7ecfe1f9840 | 82 | } |
darkumatar | 0:d7ecfe1f9840 | 83 | return true; |
darkumatar | 0:d7ecfe1f9840 | 84 | } |
darkumatar | 0:d7ecfe1f9840 | 85 | |
darkumatar | 0:d7ecfe1f9840 | 86 | //↓モーター |
darkumatar | 0:d7ecfe1f9840 | 87 | void driveMotorS(double pwm){ |
darkumatar | 3:ccd2dd7d3814 | 88 | PwmOut up_pin_A1(PA_0); |
darkumatar | 3:ccd2dd7d3814 | 89 | PwmOut up_pin_B1(PA_1); |
darkumatar | 0:d7ecfe1f9840 | 90 | up_pin_A1.period_us(2048); |
darkumatar | 0:d7ecfe1f9840 | 91 | up_pin_B1.period_us(2048); |
darkumatar | 1:d3985e5af20f | 92 | if (pwm==0) { |
darkumatar | 0:d7ecfe1f9840 | 93 | up_pin_A1 = 0; |
darkumatar | 0:d7ecfe1f9840 | 94 | up_pin_B1 = 0; |
darkumatar | 0:d7ecfe1f9840 | 95 | } else if (0 < pwm) { |
darkumatar | 0:d7ecfe1f9840 | 96 | up_pin_A1 = pwm; |
darkumatar | 0:d7ecfe1f9840 | 97 | up_pin_B1 = 0; |
darkumatar | 0:d7ecfe1f9840 | 98 | } else { |
darkumatar | 0:d7ecfe1f9840 | 99 | up_pin_A1 = 0; |
darkumatar | 0:d7ecfe1f9840 | 100 | up_pin_B1= -pwm; |
darkumatar | 0:d7ecfe1f9840 | 101 | } |
darkumatar | 0:d7ecfe1f9840 | 102 | } |
darkumatar | 3:ccd2dd7d3814 | 103 | |
darkumatar | 3:ccd2dd7d3814 | 104 | void driveMotorS3(double pwm){ |
darkumatar | 3:ccd2dd7d3814 | 105 | PwmOut up_pin_A3(PC_9); |
darkumatar | 3:ccd2dd7d3814 | 106 | PwmOut up_pin_B3(PC_8); |
darkumatar | 3:ccd2dd7d3814 | 107 | up_pin_A3.period_us(2048); |
darkumatar | 3:ccd2dd7d3814 | 108 | up_pin_B3.period_us(2048); |
darkumatar | 3:ccd2dd7d3814 | 109 | if (pwm==0) { |
darkumatar | 3:ccd2dd7d3814 | 110 | up_pin_A3 = 0; |
darkumatar | 3:ccd2dd7d3814 | 111 | up_pin_B3 = 0; |
darkumatar | 3:ccd2dd7d3814 | 112 | } else if (0 < pwm) { |
darkumatar | 3:ccd2dd7d3814 | 113 | up_pin_A3 = pwm; |
darkumatar | 3:ccd2dd7d3814 | 114 | up_pin_B3 = 0; |
darkumatar | 3:ccd2dd7d3814 | 115 | } else { |
darkumatar | 3:ccd2dd7d3814 | 116 | up_pin_A3 = 0; |
darkumatar | 3:ccd2dd7d3814 | 117 | up_pin_B3= -pwm; |
darkumatar | 3:ccd2dd7d3814 | 118 | } |
darkumatar | 3:ccd2dd7d3814 | 119 | } |
darkumatar | 3:ccd2dd7d3814 | 120 | |
darkumatar | 3:ccd2dd7d3814 | 121 | |
darkumatar | 0:d7ecfe1f9840 | 122 | //↓昇降 |
darkumatar | 0:d7ecfe1f9840 | 123 | //速さはテキトー |
darkumatar | 0:d7ecfe1f9840 | 124 | bool Up(int rx_data,int &tx_data){ |
darkumatar | 1:d3985e5af20f | 125 | |
darkumatar | 1:d3985e5af20f | 126 | if(rx_data==1){ |
darkumatar | 1:d3985e5af20f | 127 | date=1; |
darkumatar | 1:d3985e5af20f | 128 | } |
darkumatar | 1:d3985e5af20f | 129 | if(rx_data==-1){ |
darkumatar | 3:ccd2dd7d3814 | 130 | date=-1; |
darkumatar | 1:d3985e5af20f | 131 | } |
darkumatar | 1:d3985e5af20f | 132 | if(date==1 && !limitA){ |
darkumatar | 2:fe26901d9d49 | 133 | driveMotorS(0.4); |
darkumatar | 1:d3985e5af20f | 134 | }else if(date==-1 && !limitB){ |
darkumatar | 2:fe26901d9d49 | 135 | driveMotorS(-0.4); |
darkumatar | 0:d7ecfe1f9840 | 136 | }else{ |
darkumatar | 0:d7ecfe1f9840 | 137 | driveMotorS(0); |
darkumatar | 0:d7ecfe1f9840 | 138 | } |
darkumatar | 0:d7ecfe1f9840 | 139 | return true; |
darkumatar | 0:d7ecfe1f9840 | 140 | } |
darkumatar | 3:ccd2dd7d3814 | 141 | |
darkumatar | 3:ccd2dd7d3814 | 142 | //↓昇降 |
darkumatar | 3:ccd2dd7d3814 | 143 | //速さはテキトー |
darkumatar | 3:ccd2dd7d3814 | 144 | bool Up2(int rx_data,int &tx_data){ |
darkumatar | 3:ccd2dd7d3814 | 145 | |
darkumatar | 3:ccd2dd7d3814 | 146 | if(rx_data==1){ |
darkumatar | 3:ccd2dd7d3814 | 147 | date2=1; |
darkumatar | 3:ccd2dd7d3814 | 148 | } |
darkumatar | 3:ccd2dd7d3814 | 149 | if(rx_data==-1){ |
darkumatar | 3:ccd2dd7d3814 | 150 | date2=-1; |
darkumatar | 3:ccd2dd7d3814 | 151 | } |
darkumatar | 4:7e2aa008f84b | 152 | if(limitA2){ |
darkumatar | 4:7e2aa008f84b | 153 | air_date2=0; |
darkumatar | 3:ccd2dd7d3814 | 154 | } |
darkumatar | 3:ccd2dd7d3814 | 155 | |
darkumatar | 3:ccd2dd7d3814 | 156 | if(date2==1 && !limitA2){ |
darkumatar | 3:ccd2dd7d3814 | 157 | driveMotorS3(0.6); |
darkumatar | 3:ccd2dd7d3814 | 158 | limit_air=0; |
darkumatar | 4:7e2aa008f84b | 159 | }else if(tx_data==-1 &&!limitB2){ |
darkumatar | 4:7e2aa008f84b | 160 | driveMotorS3(-0.6); |
darkumatar | 3:ccd2dd7d3814 | 161 | limit_air=0; |
darkumatar | 3:ccd2dd7d3814 | 162 | }else{ |
darkumatar | 3:ccd2dd7d3814 | 163 | driveMotorS3(0); |
darkumatar | 3:ccd2dd7d3814 | 164 | } |
darkumatar | 3:ccd2dd7d3814 | 165 | return true; |
darkumatar | 3:ccd2dd7d3814 | 166 | } |
darkumatar | 3:ccd2dd7d3814 | 167 | |
darkumatar | 0:d7ecfe1f9840 | 168 | //↓回収用 |
darkumatar | 0:d7ecfe1f9840 | 169 | bool CATCH_NO(int rx_data,int &tx_data){ |
darkumatar | 1:d3985e5af20f | 170 | if(tx_data==1){ |
darkumatar | 1:d3985e5af20f | 171 | air_date+=1; |
darkumatar | 2:fe26901d9d49 | 172 | }else if(tx_data==2&&(air_date%2)==0){ |
darkumatar | 1:d3985e5af20f | 173 | air_date=1; |
darkumatar | 1:d3985e5af20f | 174 | } |
darkumatar | 1:d3985e5af20f | 175 | catch_no.move(air_date%2); |
darkumatar | 1:d3985e5af20f | 176 | return true; |
darkumatar | 2:fe26901d9d49 | 177 | } |
darkumatar | 1:d3985e5af20f | 178 | |
darkumatar | 1:d3985e5af20f | 179 | bool CATCH_NO2(int rx_data,int &tx_data){ |
darkumatar | 3:ccd2dd7d3814 | 180 | air_date2+=tx_data; |
darkumatar | 3:ccd2dd7d3814 | 181 | catch_no2.move(air_date2%2); |
darkumatar | 0:d7ecfe1f9840 | 182 | return true; |
darkumatar | 1:d3985e5af20f | 183 | |
darkumatar | 3:ccd2dd7d3814 | 184 | } |
darkumatar | 3:ccd2dd7d3814 | 185 | |
darkumatar | 1:d3985e5af20f | 186 | |
darkumatar | 1:d3985e5af20f | 187 | bool limit2(int id,int ppp){ |
darkumatar | 1:d3985e5af20f | 188 | if(ppp==1){ |
darkumatar | 1:d3985e5af20f | 189 | limit_switch=false; |
darkumatar | 1:d3985e5af20f | 190 | driveMotorS(0); |
darkumatar | 1:d3985e5af20f | 191 | driveMotorS2(0); |
darkumatar | 1:d3985e5af20f | 192 | } |
darkumatar | 1:d3985e5af20f | 193 | return true; |
darkumatar | 1:d3985e5af20f | 194 | } |
darkumatar | 1:d3985e5af20f | 195 | |
darkumatar | 1:d3985e5af20f | 196 | bool limit(int a,int &b){ |
darkumatar | 1:d3985e5af20f | 197 | return limit2(0,a); |
darkumatar | 1:d3985e5af20f | 198 | } |
darkumatar | 1:d3985e5af20f | 199 | |
darkumatar | 1:d3985e5af20f | 200 | |
darkumatar | 0:d7ecfe1f9840 | 201 | |
darkumatar | 0:d7ecfe1f9840 | 202 | int main(){ |
darkumatar | 0:d7ecfe1f9840 | 203 | timer.start(); |
darkumatar | 1:d3985e5af20f | 204 | slave.addCMD(1,limit); |
darkumatar | 1:d3985e5af20f | 205 | slave.addCMD(22,Hassya); |
darkumatar | 0:d7ecfe1f9840 | 206 | slave.addCMD(20,CATCH_NO); |
darkumatar | 0:d7ecfe1f9840 | 207 | slave.addCMD(21,Up); |
darkumatar | 3:ccd2dd7d3814 | 208 | slave.addCMD(23,CATCH_NO2); |
darkumatar | 3:ccd2dd7d3814 | 209 | slave.addCMD(24,Up2); |
darkumatar | 1:d3985e5af20f | 210 | while(limit_switch){ |
darkumatar | 1:d3985e5af20f | 211 | |
darkumatar | 1:d3985e5af20f | 212 | |
darkumatar | 1:d3985e5af20f | 213 | } |
darkumatar | 1:d3985e5af20f | 214 | driveMotorS(0); |
darkumatar | 1:d3985e5af20f | 215 | driveMotorS2(0); |
darkumatar | 0:d7ecfe1f9840 | 216 | } |