go-roboB / SPI_TEST_SERVO_AND_MOTOR

I want go to sea

Dependencies:   arrc_mbed Servo air

main.cpp@1:d3985e5af20f, 2022-03-11 (annotated)

Committer:
darkumatar
Date:
Fri Mar 11 00:45:18 2022 +0000
Revision:
1:d3985e5af20f
Parent:
0:d7ecfe1f9840
Child:
2:fe26901d9d49
ppap

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