go-roboB / SPI_TEST_SERVO_AND_MOTOR

I want go to sea

Dependencies:   arrc_mbed Servo air

main.cpp@0:d7ecfe1f9840, 2022-03-05 (annotated)

Committer:
darkumatar
Date:
Sat Mar 05 04:57:40 2022 +0000
Revision:
0:d7ecfe1f9840
Child:
1:d3985e5af20f
I want to go to bed

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 0:d7ecfe1f9840 10
darkumatar 0:d7ecfe1f9840 11 DigitalIn limitC(PB_4,PullUp);
darkumatar 0:d7ecfe1f9840 12 Air hassya(PA_8);
darkumatar 0:d7ecfe1f9840 13 Servo myservo(PB_0);
darkumatar 0:d7ecfe1f9840 14 Timer timer;
darkumatar 0:d7ecfe1f9840 15 int tim=-3000;
darkumatar 0:d7ecfe1f9840 16
darkumatar 0:d7ecfe1f9840 17
darkumatar 0:d7ecfe1f9840 18
darkumatar 0:d7ecfe1f9840 19 void driveMotorS2(double pwm){
darkumatar 0:d7ecfe1f9840 20 PwmOut up_pin_A(PB_13);
darkumatar 0:d7ecfe1f9840 21 PwmOut up_pin_B(PB_14);
darkumatar 0:d7ecfe1f9840 22 up_pin_A.period_us(2048);
darkumatar 0:d7ecfe1f9840 23 up_pin_B.period_us(2048);
darkumatar 0:d7ecfe1f9840 24 if (!pwm) {
darkumatar 0:d7ecfe1f9840 25 up_pin_A = 0;
darkumatar 0:d7ecfe1f9840 26 up_pin_B = 0;
darkumatar 0:d7ecfe1f9840 27 } else if (0 < pwm) {
darkumatar 0:d7ecfe1f9840 28 up_pin_A = pwm;
darkumatar 0:d7ecfe1f9840 29 up_pin_B = 0;
darkumatar 0:d7ecfe1f9840 30 } else {
darkumatar 0:d7ecfe1f9840 31 up_pin_A = 0;
darkumatar 0:d7ecfe1f9840 32 up_pin_B= -pwm;
darkumatar 0:d7ecfe1f9840 33 }
darkumatar 0:d7ecfe1f9840 34 }
darkumatar 0:d7ecfe1f9840 35 //　↓サーボ用
darkumatar 0:d7ecfe1f9840 36 void Servo(bool data){
darkumatar 0:d7ecfe1f9840 37 if(data==0){
darkumatar 0:d7ecfe1f9840 38 myservo = 0;
darkumatar 0:d7ecfe1f9840 39 }else{
darkumatar 0:d7ecfe1f9840 40 myservo = 0.2;
darkumatar 0:d7ecfe1f9840 41 }
darkumatar 0:d7ecfe1f9840 42 }
darkumatar 0:d7ecfe1f9840 43 //↓発射用
darkumatar 0:d7ecfe1f9840 44 bool Hassya(int rx_data,int &tx_data){
darkumatar 0:d7ecfe1f9840 45 if(rx_data == 1 && limitC==0){
darkumatar 0:d7ecfe1f9840 46 tim=timer.read_ms();
darkumatar 0:d7ecfe1f9840 47 }
darkumatar 0:d7ecfe1f9840 48 if(timer.read_ms()-tim<1000){
darkumatar 0:d7ecfe1f9840 49 hassya.move(1);
darkumatar 0:d7ecfe1f9840 50 driveMotorS2(0);
darkumatar 0:d7ecfe1f9840 51 Servo(1);
darkumatar 0:d7ecfe1f9840 52 }else if(timer.read_ms()-tim<2000){
darkumatar 0:d7ecfe1f9840 53 hassya.move(0);
darkumatar 0:d7ecfe1f9840 54 driveMotorS2(0);
darkumatar 0:d7ecfe1f9840 55 Servo(0);
darkumatar 0:d7ecfe1f9840 56
darkumatar 0:d7ecfe1f9840 57 }else if(timer.read_ms()-tim>2000 && limitC==1){
darkumatar 0:d7ecfe1f9840 58 hassya.move(0);
darkumatar 0:d7ecfe1f9840 59 driveMotorS2(0.06);
darkumatar 0:d7ecfe1f9840 60 Servo(0);
darkumatar 0:d7ecfe1f9840 61
darkumatar 0:d7ecfe1f9840 62 }else {
darkumatar 0:d7ecfe1f9840 63 hassya.move(1);
darkumatar 0:d7ecfe1f9840 64 driveMotorS2(0);
darkumatar 0:d7ecfe1f9840 65 Servo(0);
darkumatar 0:d7ecfe1f9840 66
darkumatar 0:d7ecfe1f9840 67 }
darkumatar 0:d7ecfe1f9840 68 return true;
darkumatar 0:d7ecfe1f9840 69 }
darkumatar 0:d7ecfe1f9840 70
darkumatar 0:d7ecfe1f9840 71 //↓モーター
darkumatar 0:d7ecfe1f9840 72 void driveMotorS(double pwm){
darkumatar 0:d7ecfe1f9840 73 PwmOut up_pin_A1(PA_0);
darkumatar 0:d7ecfe1f9840 74 PwmOut up_pin_B1(PA_1);
darkumatar 0:d7ecfe1f9840 75 up_pin_A1.period_us(2048);
darkumatar 0:d7ecfe1f9840 76 up_pin_B1.period_us(2048);
darkumatar 0:d7ecfe1f9840 77 if (!pwm) {
darkumatar 0:d7ecfe1f9840 78 up_pin_A1 = 0;
darkumatar 0:d7ecfe1f9840 79 up_pin_B1 = 0;
darkumatar 0:d7ecfe1f9840 80 } else if (0 < pwm) {
darkumatar 0:d7ecfe1f9840 81 up_pin_A1 = pwm;
darkumatar 0:d7ecfe1f9840 82 up_pin_B1 = 0;
darkumatar 0:d7ecfe1f9840 83 } else {
darkumatar 0:d7ecfe1f9840 84 up_pin_A1 = 0;
darkumatar 0:d7ecfe1f9840 85 up_pin_B1= -pwm;
darkumatar 0:d7ecfe1f9840 86 }
darkumatar 0:d7ecfe1f9840 87 }
darkumatar 0:d7ecfe1f9840 88 //↓昇降
darkumatar 0:d7ecfe1f9840 89 //速さはテキトー
darkumatar 0:d7ecfe1f9840 90 bool Up(int rx_data,int &tx_data){
darkumatar 0:d7ecfe1f9840 91 if(rx_data==1 && !limitA){
darkumatar 0:d7ecfe1f9840 92 driveMotorS(0.3);
darkumatar 0:d7ecfe1f9840 93 }else if(rx_data==-1 && !limitB){
darkumatar 0:d7ecfe1f9840 94 driveMotorS(-0.3);
darkumatar 0:d7ecfe1f9840 95 }else{
darkumatar 0:d7ecfe1f9840 96 driveMotorS(0);
darkumatar 0:d7ecfe1f9840 97 }
darkumatar 0:d7ecfe1f9840 98 return true;
darkumatar 0:d7ecfe1f9840 99 }
darkumatar 0:d7ecfe1f9840 100 //↓回収用
darkumatar 0:d7ecfe1f9840 101 bool CATCH_NO(int rx_data,int &tx_data){
darkumatar 0:d7ecfe1f9840 102 catch_no.move(rx_data);
darkumatar 0:d7ecfe1f9840 103 return true;
darkumatar 0:d7ecfe1f9840 104 }
darkumatar 0:d7ecfe1f9840 105
darkumatar 0:d7ecfe1f9840 106 int main(){
darkumatar 0:d7ecfe1f9840 107 timer.start();
darkumatar 0:d7ecfe1f9840 108 slave.addCMD(17,Hassya);
darkumatar 0:d7ecfe1f9840 109 slave.addCMD(20,CATCH_NO);
darkumatar 0:d7ecfe1f9840 110 slave.addCMD(21,Up);
darkumatar 0:d7ecfe1f9840 111 while(true);
darkumatar 0:d7ecfe1f9840 112 }