go-roboB / SPI_TEST_SERVO_AND_MOTOR

I want go to sea

Dependencies:   arrc_mbed Servo air

main.cpp@2:fe26901d9d49, 2022-03-12 (annotated)

Committer:
darkumatar
Date:
Sat Mar 12 01:21:35 2022 +0000
Revision:
2:fe26901d9d49
Parent:
1:d3985e5af20f
Child:
3:ccd2dd7d3814
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 0:d7ecfe1f9840 8 DigitalIn limitB(PB_12,PullUp);
darkumatar 0:d7ecfe1f9840 9 Air catch_no(PA_9);
darkumatar 2:fe26901d9d49 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 2:fe26901d9d49 21 int air_date2=0;
darkumatar 2:fe26901d9d49 22 int a=0;
darkumatar 0:d7ecfe1f9840 23
darkumatar 0:d7ecfe1f9840 24
darkumatar 0:d7ecfe1f9840 25 void driveMotorS2(double pwm){
darkumatar 0:d7ecfe1f9840 26 PwmOut up_pin_A(PB_13);
darkumatar 0:d7ecfe1f9840 27 PwmOut up_pin_B(PB_14);
darkumatar 0:d7ecfe1f9840 28 up_pin_A.period_us(2048);
darkumatar 0:d7ecfe1f9840 29 up_pin_B.period_us(2048);
darkumatar 1:d3985e5af20f 30 if (pwm==0) {
darkumatar 0:d7ecfe1f9840 31 up_pin_A = 0;
darkumatar 0:d7ecfe1f9840 32 up_pin_B = 0;
darkumatar 0:d7ecfe1f9840 33 } else if (0 < pwm) {
darkumatar 0:d7ecfe1f9840 34 up_pin_A = pwm;
darkumatar 0:d7ecfe1f9840 35 up_pin_B = 0;
darkumatar 0:d7ecfe1f9840 36 } else {
darkumatar 0:d7ecfe1f9840 37 up_pin_A = 0;
darkumatar 0:d7ecfe1f9840 38 up_pin_B= -pwm;
darkumatar 0:d7ecfe1f9840 39 }
darkumatar 0:d7ecfe1f9840 40 }
darkumatar 0:d7ecfe1f9840 41 //　↓サーボ用
darkumatar 0:d7ecfe1f9840 42 void Servo(bool data){
darkumatar 0:d7ecfe1f9840 43 if(data==0){
darkumatar 0:d7ecfe1f9840 44 myservo = 0;
darkumatar 0:d7ecfe1f9840 45 }else{
darkumatar 2:fe26901d9d49 46 myservo = 0.5;
darkumatar 0:d7ecfe1f9840 47 }
darkumatar 0:d7ecfe1f9840 48 }
darkumatar 0:d7ecfe1f9840 49 //↓発射用
darkumatar 0:d7ecfe1f9840 50 bool Hassya(int rx_data,int &tx_data){
darkumatar 2:fe26901d9d49 51 if(rx_data==1 && a==1){
darkumatar 0:d7ecfe1f9840 52 tim=timer.read_ms();
darkumatar 1:d3985e5af20f 53 a=0;
darkumatar 0:d7ecfe1f9840 54 }
darkumatar 2:fe26901d9d49 55 if(a==1 || !limitC==0){
darkumatar 2:fe26901d9d49 56 hassya.move(0);
darkumatar 2:fe26901d9d49 57 driveMotorS2(0);
darkumatar 2:fe26901d9d49 58 Servo(0);
darkumatar 2:fe26901d9d49 59 a=1;
darkumatar 1:d3985e5af20f 60 }else if(timer.read_ms()-tim<5000){
darkumatar 1:d3985e5af20f 61 hassya.move(0);
darkumatar 1:d3985e5af20f 62 driveMotorS2(0);
darkumatar 1:d3985e5af20f 63 Servo(1);
darkumatar 1:d3985e5af20f 64 }else if(timer.read_ms()-tim<6000){
darkumatar 0:d7ecfe1f9840 65 hassya.move(1);
darkumatar 0:d7ecfe1f9840 66 driveMotorS2(0);
darkumatar 1:d3985e5af20f 67 Servo(0);
darkumatar 2:fe26901d9d49 68 }else if(timer.read_ms()-tim>6000 && !limitC==1){
darkumatar 1:d3985e5af20f 69 hassya.move(1);
darkumatar 1:d3985e5af20f 70 driveMotorS2(-0.06);
darkumatar 1:d3985e5af20f 71 Servo(0);
darkumatar 2:fe26901d9d49 72 }else{
darkumatar 0:d7ecfe1f9840 73 hassya.move(0);
darkumatar 0:d7ecfe1f9840 74 driveMotorS2(0);
darkumatar 0:d7ecfe1f9840 75 Servo(0);
darkumatar 0:d7ecfe1f9840 76 }
darkumatar 0:d7ecfe1f9840 77 return true;
darkumatar 0:d7ecfe1f9840 78 }
darkumatar 0:d7ecfe1f9840 79
darkumatar 0:d7ecfe1f9840 80 //↓モーター
darkumatar 0:d7ecfe1f9840 81 void driveMotorS(double pwm){
darkumatar 1:d3985e5af20f 82 PwmOut up_pin_A1(PA_1);
darkumatar 1:d3985e5af20f 83 PwmOut up_pin_B1(PA_0);
darkumatar 0:d7ecfe1f9840 84 up_pin_A1.period_us(2048);
darkumatar 0:d7ecfe1f9840 85 up_pin_B1.period_us(2048);
darkumatar 1:d3985e5af20f 86 if (pwm==0) {
darkumatar 0:d7ecfe1f9840 87 up_pin_A1 = 0;
darkumatar 0:d7ecfe1f9840 88 up_pin_B1 = 0;
darkumatar 0:d7ecfe1f9840 89 } else if (0 < pwm) {
darkumatar 0:d7ecfe1f9840 90 up_pin_A1 = pwm;
darkumatar 0:d7ecfe1f9840 91 up_pin_B1 = 0;
darkumatar 0:d7ecfe1f9840 92 } else {
darkumatar 0:d7ecfe1f9840 93 up_pin_A1 = 0;
darkumatar 0:d7ecfe1f9840 94 up_pin_B1= -pwm;
darkumatar 0:d7ecfe1f9840 95 }
darkumatar 0:d7ecfe1f9840 96 }
darkumatar 0:d7ecfe1f9840 97 //↓昇降
darkumatar 0:d7ecfe1f9840 98 //速さはテキトー
darkumatar 0:d7ecfe1f9840 99 bool Up(int rx_data,int &tx_data){
darkumatar 1:d3985e5af20f 100
darkumatar 1:d3985e5af20f 101 if(rx_data==1){
darkumatar 1:d3985e5af20f 102 date=1;
darkumatar 1:d3985e5af20f 103 }
darkumatar 1:d3985e5af20f 104 if(rx_data==-1){
darkumatar 1:d3985e5af20f 105 date=-1 ;
darkumatar 1:d3985e5af20f 106 }
darkumatar 1:d3985e5af20f 107 if(date==1 && !limitA){
darkumatar 2:fe26901d9d49 108 driveMotorS(0.4);
darkumatar 1:d3985e5af20f 109 }else if(date==-1 && !limitB){
darkumatar 2:fe26901d9d49 110 driveMotorS(-0.4);
darkumatar 0:d7ecfe1f9840 111 }else{
darkumatar 0:d7ecfe1f9840 112 driveMotorS(0);
darkumatar 0:d7ecfe1f9840 113 }
darkumatar 0:d7ecfe1f9840 114 return true;
darkumatar 0:d7ecfe1f9840 115 }
darkumatar 0:d7ecfe1f9840 116 //↓回収用
darkumatar 0:d7ecfe1f9840 117 bool CATCH_NO(int rx_data,int &tx_data){
darkumatar 1:d3985e5af20f 118 if(tx_data==1){
darkumatar 1:d3985e5af20f 119 air_date+=1;
darkumatar 2:fe26901d9d49 120 }else if(tx_data==2&&(air_date%2)==0){
darkumatar 1:d3985e5af20f 121 air_date=1;
darkumatar 1:d3985e5af20f 122 }
darkumatar 1:d3985e5af20f 123 catch_no.move(air_date%2);
darkumatar 1:d3985e5af20f 124 return true;
darkumatar 2:fe26901d9d49 125 }
darkumatar 1:d3985e5af20f 126
darkumatar 1:d3985e5af20f 127 bool CATCH_NO2(int rx_data,int &tx_data){
darkumatar 2:fe26901d9d49 128 air_date+=tx_data;
darkumatar 2:fe26901d9d49 129 catch_no2.move(air_date%2);
darkumatar 1:d3985e5af20f 130 //catch_no3.move(rx_data);
darkumatar 0:d7ecfe1f9840 131 return true;
darkumatar 0:d7ecfe1f9840 132 }
darkumatar 1:d3985e5af20f 133
darkumatar 1:d3985e5af20f 134
darkumatar 1:d3985e5af20f 135 bool limit2(int id,int ppp){
darkumatar 1:d3985e5af20f 136 if(ppp==1){
darkumatar 1:d3985e5af20f 137 limit_switch=false;
darkumatar 1:d3985e5af20f 138 driveMotorS(0);
darkumatar 1:d3985e5af20f 139 driveMotorS2(0);
darkumatar 1:d3985e5af20f 140 }
darkumatar 1:d3985e5af20f 141 return true;
darkumatar 1:d3985e5af20f 142 }
darkumatar 1:d3985e5af20f 143
darkumatar 1:d3985e5af20f 144 bool limit(int a,int &b){
darkumatar 1:d3985e5af20f 145 return limit2(0,a);
darkumatar 1:d3985e5af20f 146 }
darkumatar 1:d3985e5af20f 147
darkumatar 1:d3985e5af20f 148
darkumatar 0:d7ecfe1f9840 149
darkumatar 0:d7ecfe1f9840 150 int main(){
darkumatar 0:d7ecfe1f9840 151 timer.start();
darkumatar 1:d3985e5af20f 152 slave.addCMD(1,limit);
darkumatar 1:d3985e5af20f 153 slave.addCMD(22,Hassya);
darkumatar 0:d7ecfe1f9840 154 slave.addCMD(20,CATCH_NO);
darkumatar 0:d7ecfe1f9840 155 slave.addCMD(21,Up);
darkumatar 1:d3985e5af20f 156 //slave.addCMD(22,CATCH_NO2);
darkumatar 1:d3985e5af20f 157 while(limit_switch){
darkumatar 1:d3985e5af20f 158
darkumatar 1:d3985e5af20f 159
darkumatar 1:d3985e5af20f 160 }
darkumatar 1:d3985e5af20f 161 driveMotorS(0);
darkumatar 1:d3985e5af20f 162 driveMotorS2(0);
darkumatar 0:d7ecfe1f9840 163 }