go-roboB / Mbed 2 deprecated go-robo2022_hassya_ras

raspai

Dependencies:   mbed SBDBT arrc_mbed Servo air

main.cpp@0:b68d87850401, 2022-02-25 (annotated)

Committer:
asumamatsumura
Date:
Fri Feb 25 04:50:23 2022 +0000
Revision:
0:b68d87850401
Child:
1:1766ac88bf57
hassya raspi you;

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 0:b68d87850401 8 Air hassya(PA_0);
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 0:b68d87850401 12 Servo myservo(PB_0);
asumamatsumura 0:b68d87850401 13 Timer timer;
asumamatsumura 0:b68d87850401 14 int tim=-3000;
asumamatsumura 0:b68d87850401 15 //↓モータ用
asumamatsumura 0:b68d87850401 16 void driveMotorS(double pwm){
asumamatsumura 0:b68d87850401 17 PwmOut up_pin_A(PB_13);
asumamatsumura 0:b68d87850401 18 PwmOut up_pin_B(PB_14);
asumamatsumura 0:b68d87850401 19 up_pin_A.period_us(2048);
asumamatsumura 0:b68d87850401 20 up_pin_B.period_us(2048);
asumamatsumura 0:b68d87850401 21 if (!pwm) {
asumamatsumura 0:b68d87850401 22 up_pin_A = 0;
asumamatsumura 0:b68d87850401 23 up_pin_B = 0;
asumamatsumura 0:b68d87850401 24 } else if (0 < pwm) {
asumamatsumura 0:b68d87850401 25 up_pin_A = pwm;
asumamatsumura 0:b68d87850401 26 up_pin_B = 0;
asumamatsumura 0:b68d87850401 27 } else {
asumamatsumura 0:b68d87850401 28 up_pin_A = 0;
asumamatsumura 0:b68d87850401 29 up_pin_B= -pwm;
asumamatsumura 0:b68d87850401 30 }
asumamatsumura 0:b68d87850401 31 }
asumamatsumura 0:b68d87850401 32 //　↓サーボ用
asumamatsumura 0:b68d87850401 33 void Servo(bool data){
asumamatsumura 0:b68d87850401 34 if(data==0){
asumamatsumura 0:b68d87850401 35 myservo = 0;
asumamatsumura 0:b68d87850401 36 }else{
asumamatsumura 0:b68d87850401 37 myservo = 0.2;
asumamatsumura 0:b68d87850401 38 }
asumamatsumura 0:b68d87850401 39 }
asumamatsumura 0:b68d87850401 40 //↓発射用
asumamatsumura 0:b68d87850401 41 bool Hassya(int rx_data,int &tx_data){
asumamatsumura 0:b68d87850401 42 if(rx_data == 1 && limitA==0){
asumamatsumura 0:b68d87850401 43 tim=timer.read_ms();
asumamatsumura 0:b68d87850401 44 }
asumamatsumura 0:b68d87850401 45 if(timer.read_ms()-tim<1000){
asumamatsumura 0:b68d87850401 46 hassya.move(1);
asumamatsumura 0:b68d87850401 47 driveMotorS(0);
asumamatsumura 0:b68d87850401 48 Servo(1);
asumamatsumura 0:b68d87850401 49 Led1.write(1);
asumamatsumura 0:b68d87850401 50 Led2.write(0);
asumamatsumura 0:b68d87850401 51 Led3.write(1);
asumamatsumura 0:b68d87850401 52 }else if(timer.read_ms()-tim<2000){
asumamatsumura 0:b68d87850401 53 hassya.move(0);
asumamatsumura 0:b68d87850401 54 driveMotorS(0);
asumamatsumura 0:b68d87850401 55 Servo(0);
asumamatsumura 0:b68d87850401 56 Led1.write(0);
asumamatsumura 0:b68d87850401 57 Led2.write(0);
asumamatsumura 0:b68d87850401 58 Led3.write(0);
asumamatsumura 0:b68d87850401 59 }else if(timer.read_ms()-tim>2000 && limitA==1){
asumamatsumura 0:b68d87850401 60 hassya.move(0);
asumamatsumura 0:b68d87850401 61 driveMotorS(0.06);
asumamatsumura 0:b68d87850401 62 Servo(0);
asumamatsumura 0:b68d87850401 63 Led1.write(0);
asumamatsumura 0:b68d87850401 64 Led2.write(1);
asumamatsumura 0:b68d87850401 65 Led3.write(0);
asumamatsumura 0:b68d87850401 66 }else {
asumamatsumura 0:b68d87850401 67 hassya.move(1);
asumamatsumura 0:b68d87850401 68 driveMotorS(0);
asumamatsumura 0:b68d87850401 69 Servo(0);
asumamatsumura 0:b68d87850401 70 Led1.write(1);
asumamatsumura 0:b68d87850401 71 Led2.write(0);
asumamatsumura 0:b68d87850401 72 Led3.write(0);
asumamatsumura 0:b68d87850401 73 }
asumamatsumura 0:b68d87850401 74 return true;
asumamatsumura 0:b68d87850401 75 }
asumamatsumura 0:b68d87850401 76 int main(){
asumamatsumura 0:b68d87850401 77 timer.start();
asumamatsumura 0:b68d87850401 78 slave.addCMD(6,Hassya);
asumamatsumura 0:b68d87850401 79 while(true);
asumamatsumura 0:b68d87850401 80 }