NagaokaRoboticsClub_mbedTeam / Mbed OS NHK2017_octopus2_drive

タコ　駆動側　

Dependencies:   2017NHKpin_config FEP R1307 PID ikarashiMDC omni_wheel

Fork of NHK2017_octopus2 by NagaokaRoboticsClub_mbedTeam

bot/bot.cpp@3:369d9ee17e84, 2017-09-05 (annotated)

Committer:
uchitake
Date:
Tue Sep 05 20:51:23 2017 +0900
Revision:
3:369d9ee17e84
Parent:
1:845af5425eec
Child:
4:1073deb368df
add func

Who changed what in which revision?

UserRevisionLine numberNew contents of line
uchitake 1:845af5425eec 1 #include "bot.h"
uchitake 1:845af5425eec 2
uchitake 1:845af5425eec 3 Bot::Bot() :
uchitake 1:845af5425eec 4 PIDC(), pad(XBee1TX, XBee1RX, ADDR), motor(MDSDA, MDSCL, solenoidPin)
uchitake 1:845af5425eec 5 {
uchitake 1:845af5425eec 6 motor.goXY(0, 0, 0);
uchitake 1:845af5425eec 7 motor.moveSlider(0);
uchitake 1:845af5425eec 8 motor.destroy(0);
uchitake 1:845af5425eec 9 motor.swing(0);
uchitake 1:845af5425eec 10 motor.shakeHead(0);
uchitake 1:845af5425eec 11 }
uchitake 1:845af5425eec 12
uchitake 1:845af5425eec 13 void Bot::confirmAll()
uchitake 1:845af5425eec 14 {
uchitake 1:845af5425eec 15 suc = pad.receiveState();
uchitake 1:845af5425eec 16 PIDC::confirm();
uchitake 1:845af5425eec 17 if(!suc) {
uchitake 1:845af5425eec 18 motor.goXY(0, 0, 0);
uchitake 1:845af5425eec 19 motor.moveSlider(0);
uchitake 1:845af5425eec 20 motor.destroy(0);
uchitake 1:845af5425eec 21 motor.swing(0);
uchitake 1:845af5425eec 22 motor.shakeHead(0);
uchitake 1:845af5425eec 23 }
uchitake 1:845af5425eec 24 }
uchitake 1:845af5425eec 25
uchitake 1:845af5425eec 26 void Bot::controllDrive()
uchitake 1:845af5425eec 27 {
uchitake 3:369d9ee17e84 28 if(suc) {
uchitake 3:369d9ee17e84 29 if(pad.getNorm(1) > 0.5) {
uchitake 3:369d9ee17e84 30 PIDC::setSetPoint((pad.getRadian(1) - M_PI / 2) * (180.0 / M_PI));
uchitake 3:369d9ee17e84 31 PIDC::confirm();
uchitake 3:369d9ee17e84 32 }
uchitake 3:369d9ee17e84 33 motor.goXY(pad.getStick(0) / 2.0,-pad.getStick(1) /2.0, PIDC::co);
uchitake 3:369d9ee17e84 34 } else {
uchitake 3:369d9ee17e84 35 motor.goXY(0, 0, 0);
uchitake 3:369d9ee17e84 36 }
uchitake 3:369d9ee17e84 37 }
uchitake 3:369d9ee17e84 38
uchitake 3:369d9ee17e84 39 void Bot::controllDrive2()
uchitake 3:369d9ee17e84 40 {
uchitake 3:369d9ee17e84 41 static float moment = 0;
uchitake 3:369d9ee17e84 42 static float beforestick = pad.getStick(2);
uchitake 3:369d9ee17e84 43
uchitake 3:369d9ee17e84 44 if((beforestick >= 0.1 && pad.getStick(2) < 0.1) || (beforestick <= -0.1 && pad.getStick(2) > -0.1)) {
uchitake 3:369d9ee17e84 45 PIDC::setSetPoint(PIDC::getDegree());
uchitake 3:369d9ee17e84 46 }
uchitake 3:369d9ee17e84 47
uchitake 3:369d9ee17e84 48 if(pad.getStick(2) > 0.1 && pad.getStick(2) < -0.1) {
uchitake 3:369d9ee17e84 49 moment = pad.getStick(2) / 2.0;
uchitake 3:369d9ee17e84 50 PIDC::confirm();
uchitake 3:369d9ee17e84 51 } else {
uchitake 3:369d9ee17e84 52 PIDC::confirm();
uchitake 3:369d9ee17e84 53 moment = PIDC::co;
uchitake 3:369d9ee17e84 54 }
uchitake 3:369d9ee17e84 55
uchitake 3:369d9ee17e84 56 if(suc) {
uchitake 3:369d9ee17e84 57 motor.goXY(pad.getStick(0) / 2.0, pad.getStick(1) / 2.0, moment);
uchitake 3:369d9ee17e84 58 } else {
uchitake 3:369d9ee17e84 59 motor.goXY(0, 0, 0);
uchitake 3:369d9ee17e84 60 }
uchitake 3:369d9ee17e84 61 beforestick = pad.getStick(2);
uchitake 1:845af5425eec 62 }
uchitake 1:845af5425eec 63
uchitake 1:845af5425eec 64 void Bot::controllMech()
uchitake 1:845af5425eec 65 {
uchitake 1:845af5425eec 66 if(suc) {
uchitake 1:845af5425eec 67 // if(!pad.getButton1(0)) motor.moveSlider(ARM_MAX_SPEED);
uchitake 1:845af5425eec 68 // if(!pad.getButton1(1)) motor.moveSlider(-ARM_MAX_SPEED);
uchitake 1:845af5425eec 69 // if(pad.getButton1(0) && pad.getButton1(1)) motor.moveSlider(0);
uchitake 1:845af5425eec 70 //
uchitake 1:845af5425eec 71 // if(!pad.getButton1(3)) motor.shakeHead(ARM_MAX_SPEED);
uchitake 1:845af5425eec 72 // if(!pad.getButton1(4)) motor.shakeHead(-ARM_MAX_SPEED);
uchitake 1:845af5425eec 73 // if(pad.getButton1(3) && pad.getButton1(4)) motor.shakeHead(0);
uchitake 1:845af5425eec 74 //
uchitake 1:845af5425eec 75 if(!pad.getButton1(5)) motor.swing(0.2);
uchitake 1:845af5425eec 76 if(!pad.getButton1(6)) motor.swing(-0.2);
uchitake 1:845af5425eec 77 if(pad.getButton1(5) && pad.getButton1(6)) motor.swing(0);
uchitake 1:845af5425eec 78
uchitake 1:845af5425eec 79 //
uchitake 1:845af5425eec 80 // if(!pad.getButton1(2)) {
uchitake 1:845af5425eec 81 // motor.destroy(DESTROY_MAX_SPEED);
uchitake 1:845af5425eec 82 // } else {
uchitake 1:845af5425eec 83 // motor.destroy(0);
uchitake 1:845af5425eec 84 // }
uchitake 1:845af5425eec 85 //
uchitake 3:369d9ee17e84 86 if(!pad.getButton2(1)) motor.release();
uchitake 3:369d9ee17e84 87 } else {
uchitake 3:369d9ee17e84 88 motor.moveSlider(0);
uchitake 3:369d9ee17e84 89 motor.destroy(0);
uchitake 3:369d9ee17e84 90 motor.swing(0);
uchitake 3:369d9ee17e84 91 motor.shakeHead(0);
uchitake 3:369d9ee17e84 92 }
uchitake 1:845af5425eec 93 }
uchitake 1:845af5425eec 94
uchitake 1:845af5425eec 95
uchitake 1:845af5425eec 96 void Bot::calibrate()
uchitake 1:845af5425eec 97 {
uchitake 3:369d9ee17e84 98 if(suc && !pad.getButton2(0) && !pad.getButton2(1)) {
uchitake 1:845af5425eec 99 PIDC::calibration(HMC6352_ENTER_CALIB);
uchitake 3:369d9ee17e84 100 motor.goXY(0, 0, 0.4);
uchitake 3:369d9ee17e84 101 wait(5.0);
uchitake 1:845af5425eec 102 motor.goXY(0, 0, 0);
uchitake 1:845af5425eec 103 PIDC::calibration(HMC6352_EXIT_CALIB);
uchitake 1:845af5425eec 104 }
uchitake 3:369d9ee17e84 105 }