NagaokaRoboticsClub_mbedTeam / Mbed OS NHK2017_octopus2_arms

タコ　腕

Dependencies:   2017NHKpin_config FEP ikarashiMDC

Fork of NHK2017_octopus2 by NagaokaRoboticsClub_mbedTeam

bot/bot.cpp@45:a3ff2bc0574b, 2017-10-27 (annotated)

Committer:
takeuchi
Date:
Fri Oct 27 16:14:50 2017 +0900
Revision:
45:a3ff2bc0574b
Parent:
44:3466b8e98fd9
Child:
46:9b714d4acdac
add slow mode

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