NagaokaRoboticsClub_mbedTeam / Mbed OS NHK2017_octopus2_arms

タコ　腕

Dependencies:   2017NHKpin_config FEP ikarashiMDC

Fork of NHK2017_octopus2 by NagaokaRoboticsClub_mbedTeam

bot/bot.cpp@6:fe9767a50891, 2017-09-06 (annotated)

Committer:
uchitake
Date:
Wed Sep 06 18:39:19 2017 +0900
Revision:
6:fe9767a50891
Parent:
5:16ea97725085
Child:
9:39be1525dfe0
fix coding

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 6:fe9767a50891 4 PIDC(),
uchitake 6:fe9767a50891 5 pad(XBee1TX, XBee1RX, ADDR),
uchitake 5:16ea97725085 6 motor(MDSDA, MDSCL, solenoidPin),
uchitake 6:fe9767a50891 7 led({DebugLED1, DebugLED2, DebugLED3, DebugLED4}),
uchitake 6:fe9767a50891 8 debugSerial(USBTX, USBRX, 11520)
uchitake 1:845af5425eec 9 {
uchitake 1:845af5425eec 10 motor.goXY(0, 0, 0);
uchitake 1:845af5425eec 11 motor.moveSlider(0);
uchitake 1:845af5425eec 12 motor.destroy(0);
uchitake 1:845af5425eec 13 motor.swing(0);
uchitake 1:845af5425eec 14 motor.shakeHead(0);
uchitake 1:845af5425eec 15 }
uchitake 1:845af5425eec 16
uchitake 1:845af5425eec 17 void Bot::confirmAll()
uchitake 1:845af5425eec 18 {
uchitake 6:fe9767a50891 19 receiveSuccessed = pad.receiveState();
uchitake 6:fe9767a50891 20 if(!receiveSuccessed) {
uchitake 1:845af5425eec 21 motor.goXY(0, 0, 0);
uchitake 1:845af5425eec 22 motor.moveSlider(0);
uchitake 1:845af5425eec 23 motor.destroy(0);
uchitake 1:845af5425eec 24 motor.swing(0);
uchitake 1:845af5425eec 25 motor.shakeHead(0);
uchitake 1:845af5425eec 26 }
uchitake 1:845af5425eec 27 }
uchitake 1:845af5425eec 28
uchitake 1:845af5425eec 29 void Bot::controllDrive()
uchitake 1:845af5425eec 30 {
uchitake 6:fe9767a50891 31 if(receiveSuccessed) {
uchitake 3:369d9ee17e84 32 if(pad.getNorm(1) > 0.5) {
uchitake 5:16ea97725085 33 PIDC::PID::setSetPoint((pad.getRadian(1) - M_PI / 2) * (180.0 / M_PI));
uchitake 3:369d9ee17e84 34 PIDC::confirm();
uchitake 3:369d9ee17e84 35 }
uchitake 6:fe9767a50891 36 motor.goXY(
uchitake 6:fe9767a50891 37 pad.getStick(0) / 2.0,
uchitake 6:fe9767a50891 38 -pad.getStick(1) /2.0,
uchitake 6:fe9767a50891 39 PIDC::calculationResult
uchitake 6:fe9767a50891 40 );
uchitake 3:369d9ee17e84 41 } else {
uchitake 3:369d9ee17e84 42 motor.goXY(0, 0, 0);
uchitake 3:369d9ee17e84 43 }
uchitake 3:369d9ee17e84 44 }
uchitake 3:369d9ee17e84 45
uchitake 3:369d9ee17e84 46 void Bot::controllDrive2()
uchitake 3:369d9ee17e84 47 {
uchitake 4:1073deb368df 48 float moment = 0;
uchitake 3:369d9ee17e84 49 static float beforestick = pad.getStick(2);
uchitake 6:fe9767a50891 50
uchitake 5:16ea97725085 51 if(!pad.getButton2(2)) {
uchitake 6:fe9767a50891 52 PIDC::resetPlaneOffset();
uchitake 5:16ea97725085 53 }
uchitake 3:369d9ee17e84 54
uchitake 4:1073deb368df 55 if((beforestick >= 0.5 && pad.getStick(2) < 0.5) || (beforestick <= -0.5 && pad.getStick(2) > -0.5)) {
uchitake 5:16ea97725085 56 PIDC::PID::setSetPoint(0.0);
uchitake 6:fe9767a50891 57 PIDC::resetAxisOffset();
uchitake 3:369d9ee17e84 58 }
uchitake 3:369d9ee17e84 59
uchitake 4:1073deb368df 60 if(pad.getStick(2) > 0.5 || pad.getStick(2) < -0.5) {
uchitake 5:16ea97725085 61 moment = pad.getStick(2) / 4.0;
uchitake 3:369d9ee17e84 62 PIDC::confirm();
uchitake 4:1073deb368df 63 }
uchitake 4:1073deb368df 64 if(fabs(pad.getStick(2)) < 0.5) {
uchitake 3:369d9ee17e84 65 PIDC::confirm();
uchitake 6:fe9767a50891 66 moment = PIDC::calculationResult;
uchitake 3:369d9ee17e84 67 }
uchitake 3:369d9ee17e84 68
uchitake 6:fe9767a50891 69 if(receiveSuccessed) {
uchitake 6:fe9767a50891 70 motor.goCircular(
uchitake 6:fe9767a50891 71 pad.getNorm(0) / 2.0,
uchitake 6:fe9767a50891 72 pad.getRadian(0) - PIDC::planeCurrentDegree / 10.0 * (M_PI / 180.0) + M_PI,
uchitake 6:fe9767a50891 73 moment
uchitake 6:fe9767a50891 74 );
uchitake 3:369d9ee17e84 75 } else {
uchitake 3:369d9ee17e84 76 motor.goXY(0, 0, 0);
uchitake 3:369d9ee17e84 77 }
uchitake 3:369d9ee17e84 78 beforestick = pad.getStick(2);
uchitake 1:845af5425eec 79 }
uchitake 1:845af5425eec 80
uchitake 1:845af5425eec 81 void Bot::controllMech()
uchitake 1:845af5425eec 82 {
uchitake 6:fe9767a50891 83 if(receiveSuccessed) {
uchitake 1:845af5425eec 84 // if(!pad.getButton1(0)) motor.moveSlider(ARM_MAX_SPEED);
uchitake 1:845af5425eec 85 // if(!pad.getButton1(1)) motor.moveSlider(-ARM_MAX_SPEED);
uchitake 1:845af5425eec 86 // if(pad.getButton1(0) && pad.getButton1(1)) motor.moveSlider(0);
uchitake 1:845af5425eec 87 //
uchitake 1:845af5425eec 88 // if(!pad.getButton1(3)) motor.shakeHead(ARM_MAX_SPEED);
uchitake 1:845af5425eec 89 // if(!pad.getButton1(4)) motor.shakeHead(-ARM_MAX_SPEED);
uchitake 1:845af5425eec 90 // if(pad.getButton1(3) && pad.getButton1(4)) motor.shakeHead(0);
uchitake 1:845af5425eec 91 //
uchitake 5:16ea97725085 92 if(!pad.getButton1(5)) motor.swing(0.6);
uchitake 5:16ea97725085 93 if(!pad.getButton1(6)) motor.swing(-0.6);
uchitake 1:845af5425eec 94 if(pad.getButton1(5) && pad.getButton1(6)) motor.swing(0);
uchitake 1:845af5425eec 95
uchitake 1:845af5425eec 96 //
uchitake 1:845af5425eec 97 // if(!pad.getButton1(2)) {
uchitake 1:845af5425eec 98 // motor.destroy(DESTROY_MAX_SPEED);
uchitake 1:845af5425eec 99 // } else {
uchitake 1:845af5425eec 100 // motor.destroy(0);
uchitake 1:845af5425eec 101 // }
uchitake 1:845af5425eec 102 //
uchitake 3:369d9ee17e84 103 if(!pad.getButton2(1)) motor.release();
uchitake 3:369d9ee17e84 104 } else {
uchitake 3:369d9ee17e84 105 motor.moveSlider(0);
uchitake 3:369d9ee17e84 106 motor.destroy(0);
uchitake 3:369d9ee17e84 107 motor.swing(0);
uchitake 3:369d9ee17e84 108 motor.shakeHead(0);
uchitake 3:369d9ee17e84 109 }
uchitake 1:845af5425eec 110 }
uchitake 1:845af5425eec 111
uchitake 1:845af5425eec 112
uchitake 1:845af5425eec 113 void Bot::calibrate()
uchitake 1:845af5425eec 114 {
uchitake 6:fe9767a50891 115 if(receiveSuccessed &&
uchitake 6:fe9767a50891 116 !pad.getButton2(0) &&
uchitake 6:fe9767a50891 117 !pad.getButton2(1)
uchitake 6:fe9767a50891 118 ) {
uchitake 1:845af5425eec 119 PIDC::calibration(HMC6352_ENTER_CALIB);
uchitake 3:369d9ee17e84 120 motor.goXY(0, 0, 0.4);
uchitake 3:369d9ee17e84 121 wait(5.0);
uchitake 1:845af5425eec 122 motor.goXY(0, 0, 0);
uchitake 1:845af5425eec 123 PIDC::calibration(HMC6352_EXIT_CALIB);
uchitake 1:845af5425eec 124 }
uchitake 3:369d9ee17e84 125 }