go-roboB / Mbed 2 deprecated go-robo2022_hassya_ras

raspai

Dependencies:   mbed SBDBT arrc_mbed Servo air

main.cpp@1:1766ac88bf57, 2022-03-04 (annotated)

Committer:
asumamatsumura
Date:
Fri Mar 04 02:52:44 2022 +0000
Revision:
1:1766ac88bf57
Parent:
0:b68d87850401
Child:
2:1d031514e4ff
kairyo

Who changed what in which revision?

UserRevisionLine numberNew contents of line
asumamatsumura 0:b68d87850401 1 #include "mbed.h"
asumamatsumura 0:b68d87850401 2 #include "air.hpp"
asumamatsumura 0:b68d87850401 3 #include "Servo.h"
asumamatsumura 0:b68d87850401 4 #include "scrp_slave.hpp"
asumamatsumura 0:b68d87850401 5 ScrpSlave slave(PC_12,PD_2,PH_1,SERIAL_TX,SERIAL_RX,0x0807ffff);
asumamatsumura 0:b68d87850401 6 Serial pc(SERIAL_TX,SERIAL_RX);
asumamatsumura 0:b68d87850401 7 DigitalIn limitA(PA_9,PullUp);
asumamatsumura 1:1766ac88bf57 8 Air hassya(PB_8);
asumamatsumura 0:b68d87850401 9 DigitalOut Led1(PA_10);
asumamatsumura 0:b68d87850401 10 DigitalOut Led2(PB_15);
asumamatsumura 0:b68d87850401 11 DigitalOut Led3(PB_2);
asumamatsumura 1:1766ac88bf57 12 Servo myservo(PB_6);
asumamatsumura 0:b68d87850401 13 Timer timer;
asumamatsumura 1:1766ac88bf57 14 int tim=-6000;
asumamatsumura 1:1766ac88bf57 15 int a=0;
asumamatsumura 0:b68d87850401 16 //↓モータ用
asumamatsumura 0:b68d87850401 17 void driveMotorS(double pwm){
asumamatsumura 0:b68d87850401 18 PwmOut up_pin_A(PB_13);
asumamatsumura 0:b68d87850401 19 PwmOut up_pin_B(PB_14);
asumamatsumura 0:b68d87850401 20 up_pin_A.period_us(2048);
asumamatsumura 0:b68d87850401 21 up_pin_B.period_us(2048);
asumamatsumura 0:b68d87850401 22 if (!pwm) {
asumamatsumura 0:b68d87850401 23 up_pin_A = 0;
asumamatsumura 0:b68d87850401 24 up_pin_B = 0;
asumamatsumura 0:b68d87850401 25 } else if (0 < pwm) {
asumamatsumura 0:b68d87850401 26 up_pin_A = pwm;
asumamatsumura 0:b68d87850401 27 up_pin_B = 0;
asumamatsumura 0:b68d87850401 28 } else {
asumamatsumura 0:b68d87850401 29 up_pin_A = 0;
asumamatsumura 0:b68d87850401 30 up_pin_B= -pwm;
asumamatsumura 0:b68d87850401 31 }
asumamatsumura 0:b68d87850401 32 }
asumamatsumura 0:b68d87850401 33 //　↓サーボ用
asumamatsumura 0:b68d87850401 34 void Servo(bool data){
asumamatsumura 0:b68d87850401 35 if(data==0){
asumamatsumura 0:b68d87850401 36 myservo = 0;
asumamatsumura 0:b68d87850401 37 }else{
asumamatsumura 1:1766ac88bf57 38 myservo = 0.5;
asumamatsumura 0:b68d87850401 39 }
asumamatsumura 0:b68d87850401 40 }
asumamatsumura 0:b68d87850401 41 //↓発射用
asumamatsumura 0:b68d87850401 42 bool Hassya(int rx_data,int &tx_data){
asumamatsumura 1:1766ac88bf57 43
asumamatsumura 1:1766ac88bf57 44 if(rx_data==1 && a==1){
asumamatsumura 0:b68d87850401 45 tim=timer.read_ms();
asumamatsumura 1:1766ac88bf57 46 a=0;
asumamatsumura 0:b68d87850401 47 }
asumamatsumura 1:1766ac88bf57 48 if(a==1 && !limitA==0){
asumamatsumura 1:1766ac88bf57 49 hassya.move(0);
asumamatsumura 1:1766ac88bf57 50 driveMotorS(0);
asumamatsumura 1:1766ac88bf57 51 Servo(0);
asumamatsumura 1:1766ac88bf57 52 Led1.write(0);
asumamatsumura 1:1766ac88bf57 53 Led2.write(0);
asumamatsumura 1:1766ac88bf57 54 Led3.write(0);
asumamatsumura 1:1766ac88bf57 55 a=1;
asumamatsumura 1:1766ac88bf57 56 }else if(timer.read_ms()-tim<5000){
asumamatsumura 1:1766ac88bf57 57 hassya.move(0);
asumamatsumura 1:1766ac88bf57 58 driveMotorS(0);
asumamatsumura 1:1766ac88bf57 59 Servo(1);
asumamatsumura 1:1766ac88bf57 60 Led1.write(0);
asumamatsumura 1:1766ac88bf57 61 Led2.write(0);
asumamatsumura 1:1766ac88bf57 62 Led3.write(1);
asumamatsumura 1:1766ac88bf57 63 }else if(timer.read_ms()-tim<6000){
asumamatsumura 0:b68d87850401 64 hassya.move(1);
asumamatsumura 0:b68d87850401 65 driveMotorS(0);
asumamatsumura 1:1766ac88bf57 66 Servo(0);
asumamatsumura 0:b68d87850401 67 Led1.write(1);
asumamatsumura 0:b68d87850401 68 Led2.write(0);
asumamatsumura 1:1766ac88bf57 69 Led3.write(0);
asumamatsumura 1:1766ac88bf57 70 }else if(timer.read_ms()-tim>6000 && !limitA==1){
asumamatsumura 1:1766ac88bf57 71 hassya.move(1);
asumamatsumura 1:1766ac88bf57 72 driveMotorS(-0.06);
asumamatsumura 1:1766ac88bf57 73 Servo(0);
asumamatsumura 1:1766ac88bf57 74 Led1.write(1);
asumamatsumura 1:1766ac88bf57 75 Led2.write(1);
asumamatsumura 1:1766ac88bf57 76 Led3.write(0);
asumamatsumura 1:1766ac88bf57 77 }else {
asumamatsumura 0:b68d87850401 78 hassya.move(0);
asumamatsumura 0:b68d87850401 79 driveMotorS(0);
asumamatsumura 0:b68d87850401 80 Servo(0);
asumamatsumura 0:b68d87850401 81 Led1.write(0);
asumamatsumura 0:b68d87850401 82 Led2.write(0);
asumamatsumura 0:b68d87850401 83 Led3.write(0);
asumamatsumura 0:b68d87850401 84 }
asumamatsumura 0:b68d87850401 85 return true;
asumamatsumura 0:b68d87850401 86 }
asumamatsumura 0:b68d87850401 87 int main(){
asumamatsumura 0:b68d87850401 88 timer.start();
asumamatsumura 0:b68d87850401 89 slave.addCMD(6,Hassya);
asumamatsumura 0:b68d87850401 90 while(true);
asumamatsumura 0:b68d87850401 91 }