NagaokaRoboticsClub_mbedTeam / Mbed OS NHK2017_octopus2_arms

タコ　腕

Dependencies:   2017NHKpin_config FEP ikarashiMDC

Fork of NHK2017_octopus2 by NagaokaRoboticsClub_mbedTeam

bot/bot.cpp@28:676330f1d186, 2017-10-03 (annotated)

Committer:
uchitake
Date:
Tue Oct 03 19:15:00 2017 +0900
Revision:
28:676330f1d186
Parent:
27:47c6eee26e76
Child:
29:41f6fc4c8962
fix drive3

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 15:9aa11febe517 5 pad(XBee2TX, XBee2RX, ADDR),
uchitake 9:39be1525dfe0 6 RS485(MDTX, MDRX, 38400),
uchitake 9:39be1525dfe0 7 RS485Controller(PWM1),
UCHITAKE 15:9aa11febe517 8 powerSwitch(MDstop),
uchitake 9:39be1525dfe0 9 quadOmni(&RS485Controller, &RS485),
uchitake 9:39be1525dfe0 10 slider(&RS485Controller, &RS485),
uchitake 9:39be1525dfe0 11 armMotor({
UCHITAKE 11:a8385ca0a275 12 ikarashiMDC({&RS485Controller, 1, 1, SM, &RS485}),
UCHITAKE 11:a8385ca0a275 13 ikarashiMDC({&RS485Controller, 1, 2, SM, &RS485}),
UCHITAKE 11:a8385ca0a275 14 ikarashiMDC({&RS485Controller, 1, 3, SM, &RS485})
uchitake 9:39be1525dfe0 15 }),
uchitake 9:39be1525dfe0 16 receiveSuccessed(0),
uchitake 28:676330f1d186 17 frontDegree(0),
uchitake 14:1fadf7d2f583 18 led({DebugLED3, DebugLED4, DebugLED5}),
uchitake 9:39be1525dfe0 19 debugSerial(USBTX, USBRX, 115200)
uchitake 1:845af5425eec 20 {
uchitake 9:39be1525dfe0 21 for(int i = 0; i < 3; i++) {
uchitake 9:39be1525dfe0 22 armMotor[i].braking = true;
uchitake 9:39be1525dfe0 23 }
uchitake 9:39be1525dfe0 24
uchitake 9:39be1525dfe0 25 quadOmni.moveXY(0, 0, 0);
uchitake 9:39be1525dfe0 26
UCHITAKE 15:9aa11febe517 27 powerSwitch = true;
uchitake 14:1fadf7d2f583 28 for(int i = 0; i < 3; i++) {
uchitake 14:1fadf7d2f583 29 led[i] = true;
uchitake 9:39be1525dfe0 30 wait(0.1);
uchitake 14:1fadf7d2f583 31 led[i] = false;
UCHITAKE 10:99dc4ae08998 32 }
uchitake 1:845af5425eec 33 }
uchitake 1:845af5425eec 34
uchitake 1:845af5425eec 35 void Bot::confirmAll()
uchitake 1:845af5425eec 36 {
uchitake 6:fe9767a50891 37 receiveSuccessed = pad.receiveState();
uchitake 16:50651ff960b9 38 if(!pad.getButton2(3) && !pad.getButton2(0)) {
uchitake 16:50651ff960b9 39 powerSwitch = 0;
uchitake 16:50651ff960b9 40 }
uchitake 14:1fadf7d2f583 41 led[0] = receiveSuccessed;
uchitake 6:fe9767a50891 42 if(!receiveSuccessed) {
uchitake 9:39be1525dfe0 43 quadOmni.moveXY(0, 0, 0);
uchitake 9:39be1525dfe0 44 slider.slide(0);
uchitake 9:39be1525dfe0 45 for(int i = 0; i < 3; i++) {
uchitake 9:39be1525dfe0 46 armMotor[i].setSpeed(0);
uchitake 9:39be1525dfe0 47 }
uchitake 1:845af5425eec 48 }
uchitake 1:845af5425eec 49 }
uchitake 1:845af5425eec 50
uchitake 1:845af5425eec 51 void Bot::controllDrive()
uchitake 1:845af5425eec 52 {
uchitake 6:fe9767a50891 53 if(receiveSuccessed) {
uchitake 17:79fa65706f92 54 debugSerial.printf("%d\n\r", PIDC::getRawDegree());
uchitake 14:1fadf7d2f583 55 led[1] = !led[1];
uchitake 3:369d9ee17e84 56 if(pad.getNorm(1) > 0.5) {
uchitake 17:79fa65706f92 57 PIDC::PID::setSetPoint((pad.getRadian(1) - M_PI / 2) * (180.0 / M_PI));
uchitake 17:79fa65706f92 58 PIDC::confirm();
uchitake 3:369d9ee17e84 59 }
uchitake 9:39be1525dfe0 60 quadOmni.moveXY(
UCHITAKE 15:9aa11febe517 61 pad.getStick(0),
UCHITAKE 15:9aa11febe517 62 -pad.getStick(1),
uchitake 24:593910fa1397 63 0,
uchitake 24:593910fa1397 64 0,
UCHITAKE 15:9aa11febe517 65 - pad.getStick(2) / 3.0 // PIDC::calculationResult
uchitake 6:fe9767a50891 66 );
uchitake 3:369d9ee17e84 67 } else {
uchitake 9:39be1525dfe0 68 quadOmni.moveXY(0, 0, 0);
uchitake 3:369d9ee17e84 69 }
uchitake 3:369d9ee17e84 70 }
uchitake 3:369d9ee17e84 71
uchitake 3:369d9ee17e84 72 void Bot::controllDrive2()
uchitake 3:369d9ee17e84 73 {
uchitake 4:1073deb368df 74 float moment = 0;
uchitake 3:369d9ee17e84 75 static float beforestick = pad.getStick(2);
uchitake 6:fe9767a50891 76
uchitake 5:16ea97725085 77 if(!pad.getButton2(2)) {
uchitake 6:fe9767a50891 78 PIDC::resetPlaneOffset();
uchitake 5:16ea97725085 79 }
uchitake 3:369d9ee17e84 80
uchitake 4:1073deb368df 81 if((beforestick >= 0.5 && pad.getStick(2) < 0.5) || (beforestick <= -0.5 && pad.getStick(2) > -0.5)) {
uchitake 5:16ea97725085 82 PIDC::PID::setSetPoint(0.0);
uchitake 6:fe9767a50891 83 PIDC::resetAxisOffset();
uchitake 3:369d9ee17e84 84 }
uchitake 3:369d9ee17e84 85
uchitake 4:1073deb368df 86 if(pad.getStick(2) > 0.5 || pad.getStick(2) < -0.5) {
uchitake 5:16ea97725085 87 moment = pad.getStick(2) / 4.0;
uchitake 3:369d9ee17e84 88 PIDC::confirm();
uchitake 4:1073deb368df 89 }
uchitake 4:1073deb368df 90 if(fabs(pad.getStick(2)) < 0.5) {
uchitake 3:369d9ee17e84 91 PIDC::confirm();
uchitake 6:fe9767a50891 92 moment = PIDC::calculationResult;
uchitake 3:369d9ee17e84 93 }
uchitake 3:369d9ee17e84 94
uchitake 6:fe9767a50891 95 if(receiveSuccessed) {
uchitake 14:1fadf7d2f583 96 led[1] = !led[1];
uchitake 9:39be1525dfe0 97 quadOmni.moveCircular(
uchitake 6:fe9767a50891 98 pad.getNorm(0) / 2.0,
uchitake 6:fe9767a50891 99 pad.getRadian(0) - PIDC::planeCurrentDegree / 10.0 * (M_PI / 180.0) + M_PI,
uchitake 6:fe9767a50891 100 moment
uchitake 6:fe9767a50891 101 );
uchitake 3:369d9ee17e84 102 } else {
uchitake 9:39be1525dfe0 103 quadOmni.moveXY(0, 0, 0);
uchitake 3:369d9ee17e84 104 }
uchitake 3:369d9ee17e84 105 beforestick = pad.getStick(2);
uchitake 1:845af5425eec 106 }
uchitake 1:845af5425eec 107
uchitake 17:79fa65706f92 108 void Bot::controllDrive3()
uchitake 17:79fa65706f92 109 {
uchitake 28:676330f1d186 110 static int rollR = 0;
uchitake 28:676330f1d186 111 static int rollL = 0;
uchitake 17:79fa65706f92 112 static int mode = 1;
uchitake 17:79fa65706f92 113 if(receiveSuccessed) {
uchitake 28:676330f1d186 114 if(rollR && !pad.getButton2(2)) {
uchitake 28:676330f1d186 115 frontDegree += ADJUST_DEGREE;
uchitake 28:676330f1d186 116 }
uchitake 28:676330f1d186 117 rollR = pad.getButton2(2);
uchitake 28:676330f1d186 118
uchitake 28:676330f1d186 119 if(rollL && !pad.getButton2(0)) {
uchitake 28:676330f1d186 120 frontDegree -= ADJUST_DEGREE;
uchitake 28:676330f1d186 121 }
uchitake 28:676330f1d186 122 rollL = pad.getButton2(0);
uchitake 17:79fa65706f92 123
uchitake 17:79fa65706f92 124 if(!pad.getButton2(4)) {
uchitake 17:79fa65706f92 125 mode = 1;
uchitake 17:79fa65706f92 126 }
uchitake 17:79fa65706f92 127
uchitake 17:79fa65706f92 128 if(!pad.getButton2(5)) {
uchitake 17:79fa65706f92 129 mode = 2;
uchitake 17:79fa65706f92 130 }
uchitake 17:79fa65706f92 131
uchitake 17:79fa65706f92 132 if(mode == 1) {
uchitake 28:676330f1d186 133 PIDC::PID::setSetPoint(frontDegree);
uchitake 17:79fa65706f92 134 PIDC::confirm();
uchitake 17:79fa65706f92 135
uchitake 17:79fa65706f92 136 quadOmni.moveXY(
uchitake 17:79fa65706f92 137 pad.getStick(0),
uchitake 17:79fa65706f92 138 -pad.getStick(1),
takeuchi 27:47c6eee26e76 139 0.5,
takeuchi 27:47c6eee26e76 140 0.5,
uchitake 17:79fa65706f92 141 -PIDC::calculationResult
uchitake 17:79fa65706f92 142 );
uchitake 17:79fa65706f92 143 }
uchitake 17:79fa65706f92 144 if(mode == 2) {
uchitake 28:676330f1d186 145 PIDC::PID::setSetPoint(90.0 + frontDegree);
uchitake 17:79fa65706f92 146 PIDC::confirm();
uchitake 17:79fa65706f92 147
uchitake 17:79fa65706f92 148 quadOmni.moveXY(
uchitake 17:79fa65706f92 149 -pad.getStick(1),
uchitake 19:34da005ea4ea 150 -pad.getStick(0),
takeuchi 27:47c6eee26e76 151 0.5,
takeuchi 27:47c6eee26e76 152 0.5,
uchitake 17:79fa65706f92 153 -PIDC::calculationResult
uchitake 17:79fa65706f92 154 );
uchitake 17:79fa65706f92 155 }
uchitake 17:79fa65706f92 156 } else {
uchitake 17:79fa65706f92 157 quadOmni.moveXY(0, 0, 0);
uchitake 17:79fa65706f92 158 }
uchitake 17:79fa65706f92 159 }
uchitake 17:79fa65706f92 160
uchitake 1:845af5425eec 161 void Bot::controllMech()
uchitake 1:845af5425eec 162 {
uchitake 6:fe9767a50891 163 if(receiveSuccessed) {
uchitake 16:50651ff960b9 164 if(!pad.getButton1(6)) {
uchitake 16:50651ff960b9 165 armMotor[SWORD].setSpeed(0.6);
uchitake 16:50651ff960b9 166 }
uchitake 16:50651ff960b9 167 if(pad.getButton1(6)) armMotor[SWORD].setSpeed(0.0);
uchitake 1:845af5425eec 168
uchitake 19:34da005ea4ea 169 slider.slide(pad.getStick(3));
UCHITAKE 15:9aa11febe517 170
uchitake 16:50651ff960b9 171 if(!pad.getButton2(1)) {
UCHITAKE 15:9aa11febe517 172 armMotor[DESTROY].setSpeed(-1.0);
UCHITAKE 15:9aa11febe517 173 } else {
UCHITAKE 15:9aa11febe517 174 armMotor[DESTROY].setSpeed(0.0);
UCHITAKE 15:9aa11febe517 175 }
uchitake 3:369d9ee17e84 176 } else {
uchitake 9:39be1525dfe0 177 slider.slide(0);
uchitake 9:39be1525dfe0 178 for(int i = 0; i < 3; i++) {
uchitake 9:39be1525dfe0 179 armMotor[i].setSpeed(0);
uchitake 9:39be1525dfe0 180 }
uchitake 3:369d9ee17e84 181 }
uchitake 1:845af5425eec 182 }
uchitake 1:845af5425eec 183
uchitake 1:845af5425eec 184
uchitake 1:845af5425eec 185 void Bot::calibrate()
uchitake 1:845af5425eec 186 {
uchitake 6:fe9767a50891 187 if(receiveSuccessed &&
uchitake 6:fe9767a50891 188 !pad.getButton2(0) &&
uchitake 6:fe9767a50891 189 !pad.getButton2(1)
uchitake 6:fe9767a50891 190 ) {
uchitake 19:34da005ea4ea 191 t.start();
uchitake 19:34da005ea4ea 192 t.reset();
uchitake 1:845af5425eec 193 PIDC::calibration(HMC6352_ENTER_CALIB);
uchitake 19:34da005ea4ea 194 while(t.read() < 5.0) {
uchitake 19:34da005ea4ea 195 quadOmni.moveXY(0, 0, 0.4);
uchitake 19:34da005ea4ea 196 slider.slide(0);
uchitake 19:34da005ea4ea 197 for(int i = 0; i < 3; i++) {
uchitake 19:34da005ea4ea 198 armMotor[i].setSpeed(0);
uchitake 19:34da005ea4ea 199 }
uchitake 19:34da005ea4ea 200 }
uchitake 19:34da005ea4ea 201 t.stop();
uchitake 9:39be1525dfe0 202 quadOmni.moveXY(0, 0, 0);
uchitake 1:845af5425eec 203 PIDC::calibration(HMC6352_EXIT_CALIB);
uchitake 1:845af5425eec 204 }
uchitake 3:369d9ee17e84 205 }