go-roboB / SPI_TEST_SERVO_AND_MOTOR

I want go to sea

Dependencies:   arrc_mbed Servo air

main.cpp@4:7e2aa008f84b, 2022-04-01 (annotated)

Committer:
darkumatar
Date:
Fri Apr 01 00:23:31 2022 +0000
Revision:
4:7e2aa008f84b
Parent:
3:ccd2dd7d3814
Child:
5:1506bf89f48e
arm

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 4:7e2aa008f84b 150 if(limitA2){
darkumatar 4:7e2aa008f84b 151 air_date2=0;
darkumatar 3:ccd2dd7d3814 152 }
darkumatar 3:ccd2dd7d3814 153
darkumatar 3:ccd2dd7d3814 154 if(date2==1 && !limitA2){
darkumatar 3:ccd2dd7d3814 155 driveMotorS3(0.6);
darkumatar 3:ccd2dd7d3814 156 limit_air=0;
darkumatar 4:7e2aa008f84b 157 }else if(tx_data==-1 &&!limitB2){
darkumatar 4:7e2aa008f84b 158 driveMotorS3(-0.6);
darkumatar 3:ccd2dd7d3814 159 limit_air=0;
darkumatar 3:ccd2dd7d3814 160 }else{
darkumatar 3:ccd2dd7d3814 161 driveMotorS3(0);
darkumatar 3:ccd2dd7d3814 162 }
darkumatar 3:ccd2dd7d3814 163 return true;
darkumatar 3:ccd2dd7d3814 164 }
darkumatar 3:ccd2dd7d3814 165
darkumatar 0:d7ecfe1f9840 166 //↓回収用
darkumatar 0:d7ecfe1f9840 167 bool CATCH_NO(int rx_data,int &tx_data){
darkumatar 1:d3985e5af20f 168 if(tx_data==1){
darkumatar 1:d3985e5af20f 169 air_date+=1;
darkumatar 2:fe26901d9d49 170 }else if(tx_data==2&&(air_date%2)==0){
darkumatar 1:d3985e5af20f 171 air_date=1;
darkumatar 1:d3985e5af20f 172 }
darkumatar 1:d3985e5af20f 173 catch_no.move(air_date%2);
darkumatar 1:d3985e5af20f 174 return true;
darkumatar 2:fe26901d9d49 175 }
darkumatar 1:d3985e5af20f 176
darkumatar 1:d3985e5af20f 177 bool CATCH_NO2(int rx_data,int &tx_data){
darkumatar 3:ccd2dd7d3814 178 air_date2+=tx_data;
darkumatar 3:ccd2dd7d3814 179 catch_no2.move(air_date2%2);
darkumatar 0:d7ecfe1f9840 180 return true;
darkumatar 1:d3985e5af20f 181
darkumatar 3:ccd2dd7d3814 182 }
darkumatar 3:ccd2dd7d3814 183
darkumatar 1:d3985e5af20f 184
darkumatar 1:d3985e5af20f 185 bool limit2(int id,int ppp){
darkumatar 1:d3985e5af20f 186 if(ppp==1){
darkumatar 1:d3985e5af20f 187 limit_switch=false;
darkumatar 1:d3985e5af20f 188 driveMotorS(0);
darkumatar 1:d3985e5af20f 189 driveMotorS2(0);
darkumatar 1:d3985e5af20f 190 }
darkumatar 1:d3985e5af20f 191 return true;
darkumatar 1:d3985e5af20f 192 }
darkumatar 1:d3985e5af20f 193
darkumatar 1:d3985e5af20f 194 bool limit(int a,int &b){
darkumatar 1:d3985e5af20f 195 return limit2(0,a);
darkumatar 1:d3985e5af20f 196 }
darkumatar 1:d3985e5af20f 197
darkumatar 1:d3985e5af20f 198
darkumatar 0:d7ecfe1f9840 199
darkumatar 0:d7ecfe1f9840 200 int main(){
darkumatar 0:d7ecfe1f9840 201 timer.start();
darkumatar 1:d3985e5af20f 202 slave.addCMD(1,limit);
darkumatar 1:d3985e5af20f 203 slave.addCMD(22,Hassya);
darkumatar 0:d7ecfe1f9840 204 slave.addCMD(20,CATCH_NO);
darkumatar 0:d7ecfe1f9840 205 slave.addCMD(21,Up);
darkumatar 3:ccd2dd7d3814 206 slave.addCMD(23,CATCH_NO2);
darkumatar 3:ccd2dd7d3814 207 slave.addCMD(24,Up2);
darkumatar 1:d3985e5af20f 208 while(limit_switch){
darkumatar 1:d3985e5af20f 209
darkumatar 1:d3985e5af20f 210
darkumatar 1:d3985e5af20f 211 }
darkumatar 1:d3985e5af20f 212 driveMotorS(0);
darkumatar 1:d3985e5af20f 213 driveMotorS2(0);
darkumatar 0:d7ecfe1f9840 214 }