NagaokaRoboticsClub_mbedTeam / Mbed OS NHK2017_octopus2_arms

タコ　腕

Dependencies:   2017NHKpin_config FEP ikarashiMDC

Fork of NHK2017_octopus2 by NagaokaRoboticsClub_mbedTeam

bot/bot.cpp@4:1073deb368df, 2017-09-06 (annotated)

Committer:
uchitake
Date:
Wed Sep 06 00:01:47 2017 +0900
Revision:
4:1073deb368df
Parent:
3:369d9ee17e84
Child:
5:16ea97725085
fix Drive 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 4:1073deb368df 4 PIDC(), pad(XBee1TX, XBee1RX, ADDR),
uchitake 4:1073deb368df 5 motor(MDSDA, MDSCL, solenoidPin)
uchitake 1:845af5425eec 6 {
uchitake 1:845af5425eec 7 motor.goXY(0, 0, 0);
uchitake 1:845af5425eec 8 motor.moveSlider(0);
uchitake 1:845af5425eec 9 motor.destroy(0);
uchitake 1:845af5425eec 10 motor.swing(0);
uchitake 1:845af5425eec 11 motor.shakeHead(0);
uchitake 1:845af5425eec 12 }
uchitake 1:845af5425eec 13
uchitake 1:845af5425eec 14 void Bot::confirmAll()
uchitake 1:845af5425eec 15 {
uchitake 1:845af5425eec 16 suc = pad.receiveState();
uchitake 1:845af5425eec 17 PIDC::confirm();
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 3:369d9ee17e84 29 if(suc) {
uchitake 3:369d9ee17e84 30 if(pad.getNorm(1) > 0.5) {
uchitake 3:369d9ee17e84 31 PIDC::setSetPoint((pad.getRadian(1) - M_PI / 2) * (180.0 / M_PI));
uchitake 3:369d9ee17e84 32 PIDC::confirm();
uchitake 3:369d9ee17e84 33 }
uchitake 3:369d9ee17e84 34 motor.goXY(pad.getStick(0) / 2.0,-pad.getStick(1) /2.0, PIDC::co);
uchitake 3:369d9ee17e84 35 } else {
uchitake 3:369d9ee17e84 36 motor.goXY(0, 0, 0);
uchitake 3:369d9ee17e84 37 }
uchitake 3:369d9ee17e84 38 }
uchitake 3:369d9ee17e84 39
uchitake 3:369d9ee17e84 40 void Bot::controllDrive2()
uchitake 3:369d9ee17e84 41 {
uchitake 4:1073deb368df 42 float moment = 0;
uchitake 3:369d9ee17e84 43 static float beforestick = pad.getStick(2);
uchitake 3:369d9ee17e84 44
uchitake 4:1073deb368df 45 if((beforestick >= 0.5 && pad.getStick(2) < 0.5) || (beforestick <= -0.5 && pad.getStick(2) > -0.5)) {
uchitake 4:1073deb368df 46 PIDC::setSetPoint(PIDC::getDegree() / 10.0);
uchitake 3:369d9ee17e84 47 }
uchitake 3:369d9ee17e84 48
uchitake 4:1073deb368df 49 if(pad.getStick(2) > 0.5 || pad.getStick(2) < -0.5) {
uchitake 3:369d9ee17e84 50 moment = pad.getStick(2) / 2.0;
uchitake 3:369d9ee17e84 51 PIDC::confirm();
uchitake 4:1073deb368df 52 }
uchitake 4:1073deb368df 53 if(fabs(pad.getStick(2)) < 0.5) {
uchitake 3:369d9ee17e84 54 PIDC::confirm();
uchitake 3:369d9ee17e84 55 moment = PIDC::co;
uchitake 3:369d9ee17e84 56 }
uchitake 3:369d9ee17e84 57
uchitake 3:369d9ee17e84 58 if(suc) {
uchitake 4:1073deb368df 59 motor.goCircular(pad.getNorm(0),pad.getRadian(0) - PIDC::getDegree() / 10.0 * (M_PI / 180.0) + M_PI, moment);
uchitake 3:369d9ee17e84 60 } else {
uchitake 3:369d9ee17e84 61 motor.goXY(0, 0, 0);
uchitake 3:369d9ee17e84 62 }
uchitake 3:369d9ee17e84 63 beforestick = pad.getStick(2);
uchitake 1:845af5425eec 64 }
uchitake 1:845af5425eec 65
uchitake 1:845af5425eec 66 void Bot::controllMech()
uchitake 1:845af5425eec 67 {
uchitake 1:845af5425eec 68 if(suc) {
uchitake 1:845af5425eec 69 // if(!pad.getButton1(0)) motor.moveSlider(ARM_MAX_SPEED);
uchitake 1:845af5425eec 70 // if(!pad.getButton1(1)) motor.moveSlider(-ARM_MAX_SPEED);
uchitake 1:845af5425eec 71 // if(pad.getButton1(0) && pad.getButton1(1)) motor.moveSlider(0);
uchitake 1:845af5425eec 72 //
uchitake 1:845af5425eec 73 // if(!pad.getButton1(3)) motor.shakeHead(ARM_MAX_SPEED);
uchitake 1:845af5425eec 74 // if(!pad.getButton1(4)) motor.shakeHead(-ARM_MAX_SPEED);
uchitake 1:845af5425eec 75 // if(pad.getButton1(3) && pad.getButton1(4)) motor.shakeHead(0);
uchitake 1:845af5425eec 76 //
uchitake 1:845af5425eec 77 if(!pad.getButton1(5)) motor.swing(0.2);
uchitake 1:845af5425eec 78 if(!pad.getButton1(6)) motor.swing(-0.2);
uchitake 1:845af5425eec 79 if(pad.getButton1(5) && pad.getButton1(6)) motor.swing(0);
uchitake 1:845af5425eec 80
uchitake 1:845af5425eec 81 //
uchitake 1:845af5425eec 82 // if(!pad.getButton1(2)) {
uchitake 1:845af5425eec 83 // motor.destroy(DESTROY_MAX_SPEED);
uchitake 1:845af5425eec 84 // } else {
uchitake 1:845af5425eec 85 // motor.destroy(0);
uchitake 1:845af5425eec 86 // }
uchitake 1:845af5425eec 87 //
uchitake 3:369d9ee17e84 88 if(!pad.getButton2(1)) motor.release();
uchitake 3:369d9ee17e84 89 } else {
uchitake 3:369d9ee17e84 90 motor.moveSlider(0);
uchitake 3:369d9ee17e84 91 motor.destroy(0);
uchitake 3:369d9ee17e84 92 motor.swing(0);
uchitake 3:369d9ee17e84 93 motor.shakeHead(0);
uchitake 3:369d9ee17e84 94 }
uchitake 1:845af5425eec 95 }
uchitake 1:845af5425eec 96
uchitake 1:845af5425eec 97
uchitake 1:845af5425eec 98 void Bot::calibrate()
uchitake 1:845af5425eec 99 {
uchitake 3:369d9ee17e84 100 if(suc && !pad.getButton2(0) && !pad.getButton2(1)) {
uchitake 1:845af5425eec 101 PIDC::calibration(HMC6352_ENTER_CALIB);
uchitake 3:369d9ee17e84 102 motor.goXY(0, 0, 0.4);
uchitake 3:369d9ee17e84 103 wait(5.0);
uchitake 1:845af5425eec 104 motor.goXY(0, 0, 0);
uchitake 1:845af5425eec 105 PIDC::calibration(HMC6352_EXIT_CALIB);
uchitake 1:845af5425eec 106 }
uchitake 3:369d9ee17e84 107 }