NagaokaRoboticsClub_mbedTeam / Mbed OS NHK2017_octopus2

Octopus!!

Dependencies:   2017NHKpin_config FEP HMC6352 PID QEI R1307 ikarashiMDC omni_wheel

Fork of KANIv3 by NagaokaRoboticsClub_mbedTeam

bot/bot.cpp@1:845af5425eec, 2017-09-05 (annotated)

Committer:
uchitake
Date:
Tue Sep 05 16:11:20 2017 +0900
Revision:
1:845af5425eec
Child:
3:369d9ee17e84
mbed-os5

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(pad.getNorm(1) > 0.5) PIDC::setSetPoint(pad.getRadian(1) * (180.0 / M_PI) - M_PI / 2.0);
uchitake 1:845af5425eec 18 if(!suc) {
uchitake 1:845af5425eec 19 motor.goXY(0, 0, 0);
uchitake 1:845af5425eec 20 motor.moveSlider(0);
uchitake 1:845af5425eec 21 motor.destroy(0);
uchitake 1:845af5425eec 22 motor.swing(0);
uchitake 1:845af5425eec 23 motor.shakeHead(0);
uchitake 1:845af5425eec 24 }
uchitake 1:845af5425eec 25 }
uchitake 1:845af5425eec 26
uchitake 1:845af5425eec 27 void Bot::controllDrive()
uchitake 1:845af5425eec 28 {
uchitake 1:845af5425eec 29 if(suc) motor.goXY(pad.getStick(0) / 2.0,-pad.getStick(1) / 2.0, PIDC::co);
uchitake 1:845af5425eec 30 }
uchitake 1:845af5425eec 31
uchitake 1:845af5425eec 32 void Bot::controllMech()
uchitake 1:845af5425eec 33 {
uchitake 1:845af5425eec 34 if(suc) {
uchitake 1:845af5425eec 35 // if(!pad.getButton1(0)) motor.moveSlider(ARM_MAX_SPEED);
uchitake 1:845af5425eec 36 // if(!pad.getButton1(1)) motor.moveSlider(-ARM_MAX_SPEED);
uchitake 1:845af5425eec 37 // if(pad.getButton1(0) && pad.getButton1(1)) motor.moveSlider(0);
uchitake 1:845af5425eec 38 //
uchitake 1:845af5425eec 39 // if(!pad.getButton1(3)) motor.shakeHead(ARM_MAX_SPEED);
uchitake 1:845af5425eec 40 // if(!pad.getButton1(4)) motor.shakeHead(-ARM_MAX_SPEED);
uchitake 1:845af5425eec 41 // if(pad.getButton1(3) && pad.getButton1(4)) motor.shakeHead(0);
uchitake 1:845af5425eec 42 //
uchitake 1:845af5425eec 43 if(!pad.getButton1(5)) motor.swing(0.2);
uchitake 1:845af5425eec 44 if(!pad.getButton1(6)) motor.swing(-0.2);
uchitake 1:845af5425eec 45 if(pad.getButton1(5) && pad.getButton1(6)) motor.swing(0);
uchitake 1:845af5425eec 46
uchitake 1:845af5425eec 47 //
uchitake 1:845af5425eec 48 // if(!pad.getButton1(2)) {
uchitake 1:845af5425eec 49 // motor.destroy(DESTROY_MAX_SPEED);
uchitake 1:845af5425eec 50 // } else {
uchitake 1:845af5425eec 51 // motor.destroy(0);
uchitake 1:845af5425eec 52 // }
uchitake 1:845af5425eec 53 //
uchitake 1:845af5425eec 54 // if(!pad.getButton2(0)) motor.release();
uchitake 1:845af5425eec 55 }
uchitake 1:845af5425eec 56 }
uchitake 1:845af5425eec 57
uchitake 1:845af5425eec 58
uchitake 1:845af5425eec 59 void Bot::calibrate()
uchitake 1:845af5425eec 60 {
uchitake 1:845af5425eec 61 if(suc && !pad.getButton2(0) && !pad.getButton2(1) && !pad.getButton2(2) && !pad.getButton2(3)) {
uchitake 1:845af5425eec 62 PIDC::calibration(HMC6352_ENTER_CALIB);
uchitake 1:845af5425eec 63 motor.goXY(0, 0, 0.3);
uchitake 1:845af5425eec 64 wait(2.0);
uchitake 1:845af5425eec 65 motor.goXY(0, 0, 0);
uchitake 1:845af5425eec 66 PIDC::calibration(HMC6352_EXIT_CALIB);
uchitake 1:845af5425eec 67 }
uchitake 1:845af5425eec 68 }