go-roboB / SPI_TEST_SERVO_AND_MOTOR

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?

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