NagaokaRoboticsClub_mbedTeam / Mbed OS NHK2017_octopus2_arms

タコ　腕

Dependencies:   2017NHKpin_config FEP ikarashiMDC

Fork of NHK2017_octopus2 by NagaokaRoboticsClub_mbedTeam

bot/bot.cpp@5:16ea97725085, 2017-09-06 (annotated)

Committer:
uchitake
Date:
Wed Sep 06 17:47:20 2017 +0900
Revision:
5:16ea97725085
Parent:
4:1073deb368df
Child:
6:fe9767a50891
working  MEGA  STUPID CODE

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 5:16ea97725085 5 motor(MDSDA, MDSCL, solenoidPin),
uchitake 5:16ea97725085 6 led({DebugLED1, DebugLED2, DebugLED3, DebugLED4})
uchitake 5:16ea97725085 7 ,debugSerial(USBTX, USBRX, 11520)
uchitake 1:845af5425eec 8 {
uchitake 1:845af5425eec 9 motor.goXY(0, 0, 0);
uchitake 1:845af5425eec 10 motor.moveSlider(0);
uchitake 1:845af5425eec 11 motor.destroy(0);
uchitake 1:845af5425eec 12 motor.swing(0);
uchitake 1:845af5425eec 13 motor.shakeHead(0);
uchitake 5:16ea97725085 14
uchitake 5:16ea97725085 15 // led[0] = 1;
uchitake 5:16ea97725085 16 // led[1] = 1;
uchitake 5:16ea97725085 17 // led[2] = 1;
uchitake 1:845af5425eec 18 }
uchitake 1:845af5425eec 19
uchitake 1:845af5425eec 20 void Bot::confirmAll()
uchitake 1:845af5425eec 21 {
uchitake 1:845af5425eec 22 suc = pad.receiveState();
uchitake 5:16ea97725085 23 // PIDC::confirm();
uchitake 1:845af5425eec 24 if(!suc) {
uchitake 1:845af5425eec 25 motor.goXY(0, 0, 0);
uchitake 1:845af5425eec 26 motor.moveSlider(0);
uchitake 1:845af5425eec 27 motor.destroy(0);
uchitake 1:845af5425eec 28 motor.swing(0);
uchitake 1:845af5425eec 29 motor.shakeHead(0);
uchitake 1:845af5425eec 30 }
uchitake 1:845af5425eec 31 }
uchitake 1:845af5425eec 32
uchitake 1:845af5425eec 33 void Bot::controllDrive()
uchitake 1:845af5425eec 34 {
uchitake 3:369d9ee17e84 35 if(suc) {
uchitake 3:369d9ee17e84 36 if(pad.getNorm(1) > 0.5) {
uchitake 5:16ea97725085 37 PIDC::PID::setSetPoint((pad.getRadian(1) - M_PI / 2) * (180.0 / M_PI));
uchitake 3:369d9ee17e84 38 PIDC::confirm();
uchitake 3:369d9ee17e84 39 }
uchitake 3:369d9ee17e84 40 motor.goXY(pad.getStick(0) / 2.0,-pad.getStick(1) /2.0, PIDC::co);
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 5:16ea97725085 50 if(!pad.getButton2(2)) {
uchitake 5:16ea97725085 51 PIDC::resetOffset2();
uchitake 5:16ea97725085 52 led[3] = 1;
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 5:16ea97725085 57 PIDC::resetOffset();
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 3:369d9ee17e84 66 moment = PIDC::co;
uchitake 3:369d9ee17e84 67 }
uchitake 3:369d9ee17e84 68
uchitake 3:369d9ee17e84 69 if(suc) {
uchitake 5:16ea97725085 70 motor.goCircular(pad.getNorm(0) / 2.0,pad.getRadian(0) - PIDC::initDegree2 / 10.0 * (M_PI / 180.0) + M_PI, moment);
uchitake 3:369d9ee17e84 71 } else {
uchitake 3:369d9ee17e84 72 motor.goXY(0, 0, 0);
uchitake 3:369d9ee17e84 73 }
uchitake 3:369d9ee17e84 74 beforestick = pad.getStick(2);
uchitake 5:16ea97725085 75 debugSerial.printf("%d\n\r", PIDC::initDegree);
uchitake 1:845af5425eec 76 }
uchitake 1:845af5425eec 77
uchitake 1:845af5425eec 78 void Bot::controllMech()
uchitake 1:845af5425eec 79 {
uchitake 1:845af5425eec 80 if(suc) {
uchitake 1:845af5425eec 81 // if(!pad.getButton1(0)) motor.moveSlider(ARM_MAX_SPEED);
uchitake 1:845af5425eec 82 // if(!pad.getButton1(1)) motor.moveSlider(-ARM_MAX_SPEED);
uchitake 1:845af5425eec 83 // if(pad.getButton1(0) && pad.getButton1(1)) motor.moveSlider(0);
uchitake 1:845af5425eec 84 //
uchitake 1:845af5425eec 85 // if(!pad.getButton1(3)) motor.shakeHead(ARM_MAX_SPEED);
uchitake 1:845af5425eec 86 // if(!pad.getButton1(4)) motor.shakeHead(-ARM_MAX_SPEED);
uchitake 1:845af5425eec 87 // if(pad.getButton1(3) && pad.getButton1(4)) motor.shakeHead(0);
uchitake 1:845af5425eec 88 //
uchitake 5:16ea97725085 89 if(!pad.getButton1(5)) motor.swing(0.6);
uchitake 5:16ea97725085 90 if(!pad.getButton1(6)) motor.swing(-0.6);
uchitake 1:845af5425eec 91 if(pad.getButton1(5) && pad.getButton1(6)) motor.swing(0);
uchitake 1:845af5425eec 92
uchitake 1:845af5425eec 93 //
uchitake 1:845af5425eec 94 // if(!pad.getButton1(2)) {
uchitake 1:845af5425eec 95 // motor.destroy(DESTROY_MAX_SPEED);
uchitake 1:845af5425eec 96 // } else {
uchitake 1:845af5425eec 97 // motor.destroy(0);
uchitake 1:845af5425eec 98 // }
uchitake 1:845af5425eec 99 //
uchitake 3:369d9ee17e84 100 if(!pad.getButton2(1)) motor.release();
uchitake 3:369d9ee17e84 101 } else {
uchitake 3:369d9ee17e84 102 motor.moveSlider(0);
uchitake 3:369d9ee17e84 103 motor.destroy(0);
uchitake 3:369d9ee17e84 104 motor.swing(0);
uchitake 3:369d9ee17e84 105 motor.shakeHead(0);
uchitake 3:369d9ee17e84 106 }
uchitake 1:845af5425eec 107 }
uchitake 1:845af5425eec 108
uchitake 1:845af5425eec 109
uchitake 1:845af5425eec 110 void Bot::calibrate()
uchitake 1:845af5425eec 111 {
uchitake 3:369d9ee17e84 112 if(suc && !pad.getButton2(0) && !pad.getButton2(1)) {
uchitake 1:845af5425eec 113 PIDC::calibration(HMC6352_ENTER_CALIB);
uchitake 3:369d9ee17e84 114 motor.goXY(0, 0, 0.4);
uchitake 3:369d9ee17e84 115 wait(5.0);
uchitake 1:845af5425eec 116 motor.goXY(0, 0, 0);
uchitake 1:845af5425eec 117 PIDC::calibration(HMC6352_EXIT_CALIB);
uchitake 1:845af5425eec 118 }
uchitake 3:369d9ee17e84 119 }