go-roboB / SPI_TEST_SERVO_AND_MOTOR

I want go to sea

Dependencies:   arrc_mbed Servo air

main.cpp@3:ccd2dd7d3814, 2022-03-19 (annotated)

Committer:
darkumatar
Date:
Sat Mar 19 02:12:50 2022 +0000
Revision:
3:ccd2dd7d3814
Parent:
2:fe26901d9d49
Child:
4:7e2aa008f84b
o

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 3:ccd2dd7d3814 61 if(!limitC==0){
darkumatar 2:fe26901d9d49 62 a=1;
darkumatar 3:ccd2dd7d3814 63 }
darkumatar 3:ccd2dd7d3814 64 if(timer.read_ms()-tim<1000){
darkumatar 1:d3985e5af20f 65 hassya.move(0);
darkumatar 1:d3985e5af20f 66 driveMotorS2(0);
darkumatar 1:d3985e5af20f 67 Servo(1);
darkumatar 3:ccd2dd7d3814 68 }else if(timer.read_ms()-tim<2000){
darkumatar 0:d7ecfe1f9840 69 hassya.move(1);
darkumatar 0:d7ecfe1f9840 70 driveMotorS2(0);
darkumatar 1:d3985e5af20f 71 Servo(0);
darkumatar 3:ccd2dd7d3814 72 }else if(timer.read_ms()-tim>2000 && a==0){
darkumatar 1:d3985e5af20f 73 hassya.move(1);
darkumatar 3:ccd2dd7d3814 74 driveMotorS2(-0.15);
darkumatar 1:d3985e5af20f 75 Servo(0);
darkumatar 2:fe26901d9d49 76 }else{
darkumatar 0:d7ecfe1f9840 77 hassya.move(0);
darkumatar 0:d7ecfe1f9840 78 driveMotorS2(0);
darkumatar 0:d7ecfe1f9840 79 Servo(0);
darkumatar 0:d7ecfe1f9840 80 }
darkumatar 0:d7ecfe1f9840 81 return true;
darkumatar 0:d7ecfe1f9840 82 }
darkumatar 0:d7ecfe1f9840 83
darkumatar 0:d7ecfe1f9840 84 //↓モーター
darkumatar 0:d7ecfe1f9840 85 void driveMotorS(double pwm){
darkumatar 3:ccd2dd7d3814 86 PwmOut up_pin_A1(PA_0);
darkumatar 3:ccd2dd7d3814 87 PwmOut up_pin_B1(PA_1);
darkumatar 0:d7ecfe1f9840 88 up_pin_A1.period_us(2048);
darkumatar 0:d7ecfe1f9840 89 up_pin_B1.period_us(2048);
darkumatar 1:d3985e5af20f 90 if (pwm==0) {
darkumatar 0:d7ecfe1f9840 91 up_pin_A1 = 0;
darkumatar 0:d7ecfe1f9840 92 up_pin_B1 = 0;
darkumatar 0:d7ecfe1f9840 93 } else if (0 < pwm) {
darkumatar 0:d7ecfe1f9840 94 up_pin_A1 = pwm;
darkumatar 0:d7ecfe1f9840 95 up_pin_B1 = 0;
darkumatar 0:d7ecfe1f9840 96 } else {
darkumatar 0:d7ecfe1f9840 97 up_pin_A1 = 0;
darkumatar 0:d7ecfe1f9840 98 up_pin_B1= -pwm;
darkumatar 0:d7ecfe1f9840 99 }
darkumatar 0:d7ecfe1f9840 100 }
darkumatar 3:ccd2dd7d3814 101
darkumatar 3:ccd2dd7d3814 102 void driveMotorS3(double pwm){
darkumatar 3:ccd2dd7d3814 103 PwmOut up_pin_A3(PC_9);
darkumatar 3:ccd2dd7d3814 104 PwmOut up_pin_B3(PC_8);
darkumatar 3:ccd2dd7d3814 105 up_pin_A3.period_us(2048);
darkumatar 3:ccd2dd7d3814 106 up_pin_B3.period_us(2048);
darkumatar 3:ccd2dd7d3814 107 if (pwm==0) {
darkumatar 3:ccd2dd7d3814 108 up_pin_A3 = 0;
darkumatar 3:ccd2dd7d3814 109 up_pin_B3 = 0;
darkumatar 3:ccd2dd7d3814 110 } else if (0 < pwm) {
darkumatar 3:ccd2dd7d3814 111 up_pin_A3 = pwm;
darkumatar 3:ccd2dd7d3814 112 up_pin_B3 = 0;
darkumatar 3:ccd2dd7d3814 113 } else {
darkumatar 3:ccd2dd7d3814 114 up_pin_A3 = 0;
darkumatar 3:ccd2dd7d3814 115 up_pin_B3= -pwm;
darkumatar 3:ccd2dd7d3814 116 }
darkumatar 3:ccd2dd7d3814 117 }
darkumatar 3:ccd2dd7d3814 118
darkumatar 3:ccd2dd7d3814 119
darkumatar 0:d7ecfe1f9840 120 //↓昇降
darkumatar 0:d7ecfe1f9840 121 //速さはテキトー
darkumatar 0:d7ecfe1f9840 122 bool Up(int rx_data,int &tx_data){
darkumatar 1:d3985e5af20f 123
darkumatar 1:d3985e5af20f 124 if(rx_data==1){
darkumatar 1:d3985e5af20f 125 date=1;
darkumatar 1:d3985e5af20f 126 }
darkumatar 1:d3985e5af20f 127 if(rx_data==-1){
darkumatar 3:ccd2dd7d3814 128 date=-1;
darkumatar 1:d3985e5af20f 129 }
darkumatar 1:d3985e5af20f 130 if(date==1 && !limitA){
darkumatar 2:fe26901d9d49 131 driveMotorS(0.4);
darkumatar 1:d3985e5af20f 132 }else if(date==-1 && !limitB){
darkumatar 2:fe26901d9d49 133 driveMotorS(-0.4);
darkumatar 0:d7ecfe1f9840 134 }else{
darkumatar 0:d7ecfe1f9840 135 driveMotorS(0);
darkumatar 0:d7ecfe1f9840 136 }
darkumatar 0:d7ecfe1f9840 137 return true;
darkumatar 0:d7ecfe1f9840 138 }
darkumatar 3:ccd2dd7d3814 139
darkumatar 3:ccd2dd7d3814 140 //↓昇降
darkumatar 3:ccd2dd7d3814 141 //速さはテキトー
darkumatar 3:ccd2dd7d3814 142 bool Up2(int rx_data,int &tx_data){
darkumatar 3:ccd2dd7d3814 143
darkumatar 3:ccd2dd7d3814 144 if(rx_data==1){
darkumatar 3:ccd2dd7d3814 145 date2=1;
darkumatar 3:ccd2dd7d3814 146 }
darkumatar 3:ccd2dd7d3814 147 if(rx_data==-1){
darkumatar 3:ccd2dd7d3814 148 date2=-1;
darkumatar 3:ccd2dd7d3814 149 }
darkumatar 3:ccd2dd7d3814 150 if(!limitA2){
darkumatar 3:ccd2dd7d3814 151 limit_air=1;
darkumatar 3:ccd2dd7d3814 152 }
darkumatar 3:ccd2dd7d3814 153 if(!limitB2){
darkumatar 3:ccd2dd7d3814 154 limit_air=-1;
darkumatar 3:ccd2dd7d3814 155 }
darkumatar 3:ccd2dd7d3814 156
darkumatar 3:ccd2dd7d3814 157 if(date2==1 && !limitA2){
darkumatar 3:ccd2dd7d3814 158 driveMotorS3(0.6);
darkumatar 3:ccd2dd7d3814 159 limit_air=0;
darkumatar 3:ccd2dd7d3814 160 }else if(date2==-1 &&!limitB2){
darkumatar 3:ccd2dd7d3814 161 driveMotorS3(-0.4);
darkumatar 3:ccd2dd7d3814 162 limit_air=0;
darkumatar 3:ccd2dd7d3814 163 }else{
darkumatar 3:ccd2dd7d3814 164 driveMotorS3(0);
darkumatar 3:ccd2dd7d3814 165 }
darkumatar 3:ccd2dd7d3814 166 return true;
darkumatar 3:ccd2dd7d3814 167 }
darkumatar 3:ccd2dd7d3814 168
darkumatar 0:d7ecfe1f9840 169 //↓回収用
darkumatar 0:d7ecfe1f9840 170 bool CATCH_NO(int rx_data,int &tx_data){
darkumatar 1:d3985e5af20f 171 if(tx_data==1){
darkumatar 1:d3985e5af20f 172 air_date+=1;
darkumatar 2:fe26901d9d49 173 }else if(tx_data==2&&(air_date%2)==0){
darkumatar 1:d3985e5af20f 174 air_date=1;
darkumatar 1:d3985e5af20f 175 }
darkumatar 1:d3985e5af20f 176 catch_no.move(air_date%2);
darkumatar 1:d3985e5af20f 177 return true;
darkumatar 2:fe26901d9d49 178 }
darkumatar 1:d3985e5af20f 179
darkumatar 1:d3985e5af20f 180 bool CATCH_NO2(int rx_data,int &tx_data){
darkumatar 3:ccd2dd7d3814 181 air_date2+=tx_data;
darkumatar 3:ccd2dd7d3814 182 catch_no2.move(air_date2%2);
darkumatar 0:d7ecfe1f9840 183 return true;
darkumatar 1:d3985e5af20f 184
darkumatar 3:ccd2dd7d3814 185 }
darkumatar 3:ccd2dd7d3814 186
darkumatar 1:d3985e5af20f 187
darkumatar 1:d3985e5af20f 188 bool limit2(int id,int ppp){
darkumatar 1:d3985e5af20f 189 if(ppp==1){
darkumatar 1:d3985e5af20f 190 limit_switch=false;
darkumatar 1:d3985e5af20f 191 driveMotorS(0);
darkumatar 1:d3985e5af20f 192 driveMotorS2(0);
darkumatar 1:d3985e5af20f 193 }
darkumatar 1:d3985e5af20f 194 return true;
darkumatar 1:d3985e5af20f 195 }
darkumatar 1:d3985e5af20f 196
darkumatar 1:d3985e5af20f 197 bool limit(int a,int &b){
darkumatar 1:d3985e5af20f 198 return limit2(0,a);
darkumatar 1:d3985e5af20f 199 }
darkumatar 1:d3985e5af20f 200
darkumatar 1:d3985e5af20f 201
darkumatar 0:d7ecfe1f9840 202
darkumatar 0:d7ecfe1f9840 203 int main(){
darkumatar 0:d7ecfe1f9840 204 timer.start();
darkumatar 1:d3985e5af20f 205 slave.addCMD(1,limit);
darkumatar 1:d3985e5af20f 206 slave.addCMD(22,Hassya);
darkumatar 0:d7ecfe1f9840 207 slave.addCMD(20,CATCH_NO);
darkumatar 0:d7ecfe1f9840 208 slave.addCMD(21,Up);
darkumatar 3:ccd2dd7d3814 209 slave.addCMD(23,CATCH_NO2);
darkumatar 3:ccd2dd7d3814 210 slave.addCMD(24,Up2);
darkumatar 1:d3985e5af20f 211 while(limit_switch){
darkumatar 1:d3985e5af20f 212
darkumatar 1:d3985e5af20f 213
darkumatar 1:d3985e5af20f 214 }
darkumatar 1:d3985e5af20f 215 driveMotorS(0);
darkumatar 1:d3985e5af20f 216 driveMotorS2(0);
darkumatar 0:d7ecfe1f9840 217 }