NagaokaRoboticsClub_mbedTeam / Mbed OS NHK2017_octopus2_arms

タコ　腕

Dependencies:   2017NHKpin_config FEP ikarashiMDC

Fork of NHK2017_octopus2 by NagaokaRoboticsClub_mbedTeam

bot/bot.cpp@52:320f910ca6ca, 2017-11-23 (annotated)

Committer:
number_key
Date:
Thu Nov 23 11:49:39 2017 +0900
Revision:
52:320f910ca6ca
Parent:
50:3a7c858aa0f9
Child:
54:857390145ac4
dirty

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() :
number_key 49:69a7235d837a 4 pad1(XBee1TX, XBee1RX, ADDR1),
number_key 49:69a7235d837a 5 pad2(XBee2TX, XBee2RX, ADDR2),
number_key 50:3a7c858aa0f9 6 RS485(MDTX, MDRX, 115200),
uchitake 9:39be1525dfe0 7 RS485Controller(PWM1),
UCHITAKE 15:9aa11febe517 8 powerSwitch(MDstop),
uchitake 9:39be1525dfe0 9 quadOmni(&RS485Controller, &RS485),
number_key 49:69a7235d837a 10 tentacle(&RS485Controller, &RS485),
number_key 49:69a7235d837a 11 nishijoSword(&RS485Controller, &RS485),
number_key 49:69a7235d837a 12 nishijo(&RS485Controller, &RS485),
takeuchi 47:43f55ff8916b 13 plane(),
takeuchi 47:43f55ff8916b 14 axis(),
number_key 49:69a7235d837a 15 receiveSuccessed1(0),
number_key 49:69a7235d837a 16 receiveSuccessed2(0),
uchitake 28:676330f1d186 17 frontDegree(0),
uchitake 9:39be1525dfe0 18 debugSerial(USBTX, USBRX, 115200)
uchitake 1:845af5425eec 19 {
takeuchi 47:43f55ff8916b 20 debugSerial.printf("OK\r\n");
uchitake 9:39be1525dfe0 21
uchitake 9:39be1525dfe0 22 quadOmni.moveXY(0, 0, 0);
number_key 49:69a7235d837a 23 tentacle.stop();
number_key 49:69a7235d837a 24 nishijoSword.stop();
number_key 49:69a7235d837a 25 nishijo.stop();
number_key 49:69a7235d837a 26
uchitake 9:39be1525dfe0 27
UCHITAKE 15:9aa11febe517 28 powerSwitch = true;
uchitake 1:845af5425eec 29 }
uchitake 1:845af5425eec 30
number_key 49:69a7235d837a 31 void Bot::confirmPad1()
uchitake 1:845af5425eec 32 {
number_key 49:69a7235d837a 33 receiveSuccessed1 = pad1.receiveState();
number_key 50:3a7c858aa0f9 34 if(!pad1.getButton2(0) && !pad1.getButton2(1)) {
uchitake 16:50651ff960b9 35 powerSwitch = 0;
uchitake 16:50651ff960b9 36 }
number_key 50:3a7c858aa0f9 37 if(!pad1.getButton2(2) && !pad1.getButton2(3)) {
number_key 50:3a7c858aa0f9 38 powerSwitch = 1;
number_key 50:3a7c858aa0f9 39 }
number_key 49:69a7235d837a 40 if(!receiveSuccessed1) {
uchitake 9:39be1525dfe0 41 quadOmni.moveXY(0, 0, 0);
number_key 49:69a7235d837a 42 }
number_key 49:69a7235d837a 43 }
number_key 49:69a7235d837a 44
number_key 49:69a7235d837a 45 void Bot::confirmPad2()
number_key 49:69a7235d837a 46 {
number_key 49:69a7235d837a 47 receiveSuccessed2 = pad2.receiveState();
number_key 49:69a7235d837a 48 if(!pad2.getButton2(3) && !pad2.getButton2(0)) {
number_key 49:69a7235d837a 49 powerSwitch = 0;
number_key 49:69a7235d837a 50 }
number_key 49:69a7235d837a 51 if(!receiveSuccessed2) {
number_key 49:69a7235d837a 52 tentacle.stop();
number_key 49:69a7235d837a 53 nishijoSword.stop();
number_key 49:69a7235d837a 54 nishijo.stop();
uchitake 1:845af5425eec 55 }
uchitake 1:845af5425eec 56 }
uchitake 1:845af5425eec 57
uchitake 1:845af5425eec 58 void Bot::controllDrive()
uchitake 1:845af5425eec 59 {
number_key 49:69a7235d837a 60 if(receiveSuccessed1) {
number_key 49:69a7235d837a 61 float moment = 0;
number_key 49:69a7235d837a 62 //debugSerial.printf("%d\n\r", plane.getRawDegree());
number_key 49:69a7235d837a 63 /*if(pad1.getNorm(0) > 0.5) {
number_key 49:69a7235d837a 64 plane.setPoint((pad1.getRadian(0) - M_PI / 2) * (180.0 / M_PI));
takeuchi 32:b619c7787dc3 65 plane.confirm();
number_key 49:69a7235d837a 66 }*/
number_key 49:69a7235d837a 67 plane.confirm();
number_key 49:69a7235d837a 68 if(pad1.getStick(0)) {
number_key 49:69a7235d837a 69 moment = -pad1.getStick(0) / 2.0;
number_key 49:69a7235d837a 70 plane.resetOffset();
number_key 49:69a7235d837a 71 } else {
number_key 49:69a7235d837a 72 moment = -plane.getCalculationResult();
uchitake 3:369d9ee17e84 73 }
number_key 49:69a7235d837a 74 debugSerial.printf("%f\r\n",moment);
uchitake 9:39be1525dfe0 75 quadOmni.moveXY(
number_key 49:69a7235d837a 76 pad1.getStick(2),
number_key 49:69a7235d837a 77 -pad1.getStick(3),
takeuchi 37:6b6616008e78 78 0,
takeuchi 37:6b6616008e78 79 0,
number_key 49:69a7235d837a 80 moment
takeuchi 37:6b6616008e78 81 );
uchitake 3:369d9ee17e84 82 } else {
uchitake 9:39be1525dfe0 83 quadOmni.moveXY(0, 0, 0);
uchitake 3:369d9ee17e84 84 }
uchitake 3:369d9ee17e84 85 }
uchitake 3:369d9ee17e84 86
uchitake 3:369d9ee17e84 87 void Bot::controllDrive2()
uchitake 3:369d9ee17e84 88 {
number_key 50:3a7c858aa0f9 89 plane.confirm();
number_key 50:3a7c858aa0f9 90 axis.confirm();
number_key 50:3a7c858aa0f9 91 float moment = 0;
number_key 50:3a7c858aa0f9 92 float norm = 0;
number_key 50:3a7c858aa0f9 93 static float beforestick = pad1.getStick(0);
uchitake 6:fe9767a50891 94
number_key 50:3a7c858aa0f9 95 if(!pad1.getButton2(5)) {
number_key 50:3a7c858aa0f9 96 axis.resetOffset();
number_key 52:320f910ca6ca 97 //debugSerial.printf("Force RESeT\n\r");
number_key 50:3a7c858aa0f9 98 }
uchitake 3:369d9ee17e84 99
number_key 50:3a7c858aa0f9 100 if((beforestick >= 0.2 && pad1.getStick(0) < 0.2) || (beforestick <= -0.2 && pad1.getStick(0) > -0.2)) {
number_key 50:3a7c858aa0f9 101 plane.setPoint(0.0);
number_key 50:3a7c858aa0f9 102 plane.resetOffset();
number_key 50:3a7c858aa0f9 103 }
number_key 50:3a7c858aa0f9 104 /*if(!pad1.getButton2(1)&&pad1.getButton2(3)) {
number_key 50:3a7c858aa0f9 105 plane.setPoint((M_PI / 4)*(180.0 / M_PI));
number_key 50:3a7c858aa0f9 106 } else if(pad1.getButton2(1)&&!pad1.getButton2(3)){
number_key 50:3a7c858aa0f9 107 plane.setPoint(-(M_PI / 4)*(180.0 / M_PI));
number_key 50:3a7c858aa0f9 108 } else {
number_key 50:3a7c858aa0f9 109 plane.setPoint(0.0);
number_key 50:3a7c858aa0f9 110 }*/
number_key 50:3a7c858aa0f9 111 if(pad1.getStick(0) > 0.2 || pad1.getStick(0) < -0.2) {
number_key 50:3a7c858aa0f9 112 moment = pad1.getStick(0) / 2.0;
number_key 50:3a7c858aa0f9 113 } else {
number_key 50:3a7c858aa0f9 114 moment = plane.getCalculationResult();
number_key 50:3a7c858aa0f9 115
number_key 50:3a7c858aa0f9 116 }
number_key 50:3a7c858aa0f9 117 if(pad1.getButton1(0)) {
number_key 50:3a7c858aa0f9 118 norm = pad1.getNorm(1);
number_key 50:3a7c858aa0f9 119 } else {
number_key 50:3a7c858aa0f9 120 norm = pad1.getNorm(1) / 2.0;
number_key 50:3a7c858aa0f9 121 }
number_key 50:3a7c858aa0f9 122 if(receiveSuccessed1) {
number_key 50:3a7c858aa0f9 123 quadOmni.moveCircular(
number_key 50:3a7c858aa0f9 124 norm,
number_key 50:3a7c858aa0f9 125 pad1.getRadian(1) - axis.getCurrentDegree() /1.0 * (M_PI / 180.0) + M_PI,
number_key 50:3a7c858aa0f9 126 0.0,
number_key 50:3a7c858aa0f9 127 0.0,
number_key 50:3a7c858aa0f9 128 -moment
number_key 50:3a7c858aa0f9 129 );
number_key 50:3a7c858aa0f9 130 } else {
number_key 50:3a7c858aa0f9 131 quadOmni.moveXY(0, 0, 0);
number_key 50:3a7c858aa0f9 132 }
number_key 50:3a7c858aa0f9 133 beforestick = pad1.getStick(0);
uchitake 1:845af5425eec 134 }
uchitake 1:845af5425eec 135
uchitake 17:79fa65706f92 136 void Bot::controllDrive3()
uchitake 17:79fa65706f92 137 {
uchitake 28:676330f1d186 138 static int rollR = 0;
uchitake 28:676330f1d186 139 static int rollL = 0;
uchitake 17:79fa65706f92 140 static int mode = 1;
number_key 49:69a7235d837a 141 if(receiveSuccessed1) {
number_key 49:69a7235d837a 142 if(rollR && !pad1.getButton2(2)) {
uchitake 28:676330f1d186 143 frontDegree += ADJUST_DEGREE;
uchitake 28:676330f1d186 144 }
number_key 49:69a7235d837a 145 rollR = pad1.getButton2(2);
uchitake 28:676330f1d186 146
number_key 49:69a7235d837a 147 if(rollL && !pad1.getButton2(0)) {
uchitake 28:676330f1d186 148 frontDegree -= ADJUST_DEGREE;
uchitake 28:676330f1d186 149 }
number_key 49:69a7235d837a 150 rollL = pad1.getButton2(0);
uchitake 17:79fa65706f92 151
number_key 49:69a7235d837a 152 if(!pad1.getButton2(4)) {
uchitake 17:79fa65706f92 153 mode = 1;
uchitake 17:79fa65706f92 154 }
uchitake 17:79fa65706f92 155
number_key 49:69a7235d837a 156 if(!pad1.getButton2(5)) {
uchitake 17:79fa65706f92 157 mode = 2;
uchitake 17:79fa65706f92 158 }
uchitake 17:79fa65706f92 159
uchitake 17:79fa65706f92 160 if(mode == 1) {
takeuchi 32:b619c7787dc3 161 plane.setPoint(frontDegree);
takeuchi 32:b619c7787dc3 162 plane.confirm();
uchitake 17:79fa65706f92 163
uchitake 17:79fa65706f92 164 quadOmni.moveXY(
number_key 49:69a7235d837a 165 pad1.getStick(0),
number_key 49:69a7235d837a 166 -pad1.getStick(1),
takeuchi 37:6b6616008e78 167 0.5,
takeuchi 37:6b6616008e78 168 0.5,
takeuchi 37:6b6616008e78 169 -plane.getCalculationResult()
takeuchi 37:6b6616008e78 170 );
uchitake 17:79fa65706f92 171 }
uchitake 17:79fa65706f92 172 if(mode == 2) {
takeuchi 32:b619c7787dc3 173 plane.setPoint(90.0 + frontDegree);
takeuchi 32:b619c7787dc3 174 plane.confirm();
uchitake 17:79fa65706f92 175
uchitake 17:79fa65706f92 176 quadOmni.moveXY(
number_key 49:69a7235d837a 177 -pad1.getStick(1),
number_key 49:69a7235d837a 178 -pad1.getStick(0),
takeuchi 37:6b6616008e78 179 0.5,
takeuchi 37:6b6616008e78 180 0.5,
takeuchi 37:6b6616008e78 181 -plane.getCalculationResult()
takeuchi 37:6b6616008e78 182 );
uchitake 17:79fa65706f92 183 }
uchitake 17:79fa65706f92 184 } else {
uchitake 17:79fa65706f92 185 quadOmni.moveXY(0, 0, 0);
uchitake 17:79fa65706f92 186 }
uchitake 17:79fa65706f92 187 }
uchitake 17:79fa65706f92 188
takeuchi 41:ae6f844facb1 189 void Bot::controllDrive4()
takeuchi 41:ae6f844facb1 190 {
number_key 49:69a7235d837a 191 if(receiveSuccessed1) {
takeuchi 41:ae6f844facb1 192 quadOmni.moveXY(
number_key 49:69a7235d837a 193 pad1.getStick(2),
number_key 49:69a7235d837a 194 pad1.getStick(3),
takeuchi 41:ae6f844facb1 195 0.0,
takeuchi 41:ae6f844facb1 196 0.0,
number_key 49:69a7235d837a 197 -pad1.getStick(0)/2.0
takeuchi 41:ae6f844facb1 198 );
takeuchi 41:ae6f844facb1 199 } else {
takeuchi 41:ae6f844facb1 200 quadOmni.moveXY(0, 0, 0);
takeuchi 41:ae6f844facb1 201 }
takeuchi 41:ae6f844facb1 202 }
uchitake 1:845af5425eec 203 void Bot::controllMech()
uchitake 1:845af5425eec 204 {
number_key 49:69a7235d837a 205 if(receiveSuccessed2) {
number_key 49:69a7235d837a 206
number_key 49:69a7235d837a 207 //if(!pad2.getButton1(2)&&!pad2.getButton1(4)) powerSwitch=0;
number_key 49:69a7235d837a 208 tentacle.leftMove(pad2.getStick(3));
number_key 49:69a7235d837a 209 tentacle.rightMove(pad2.getStick(1));
number_key 49:69a7235d837a 210
number_key 49:69a7235d837a 211 if (!pad2.getButton2(0)) nishijoSword.move(-WIND_UP_SPEED);
number_key 49:69a7235d837a 212 if (!pad2.getButton2(1)) nishijoSword.move(WIND_UP_SPEED);
number_key 49:69a7235d837a 213 if(pad2.getButton2(0) && pad2.getButton2(1)) nishijoSword.move(0);
takeuchi 36:c1398ea8f604 214
number_key 52:320f910ca6ca 215 if (!pad2.getButton2(3)) nishijo.move(-SWORD_SPEED);
number_key 52:320f910ca6ca 216 if (!pad2.getButton2(2)) nishijo.move(SWORD_SPEED);
number_key 49:69a7235d837a 217 if(pad2.getButton2(2) && pad2.getButton2(3)) nishijo.move(0);
number_key 49:69a7235d837a 218 } else {
number_key 49:69a7235d837a 219 tentacle.stop();
number_key 49:69a7235d837a 220 nishijoSword.stop();
number_key 49:69a7235d837a 221 nishijo.stop();
number_key 49:69a7235d837a 222 }
uchitake 1:845af5425eec 223 }
uchitake 1:845af5425eec 224
uchitake 1:845af5425eec 225 void Bot::calibrate()
uchitake 1:845af5425eec 226 {
uchitake 3:369d9ee17e84 227 }
number_key 49:69a7235d837a 228
number_key 49:69a7235d837a 229 void Bot::checkConnection()
number_key 49:69a7235d837a 230 {
number_key 52:320f910ca6ca 231 if(receiveSuccessed1 == 1) debugSerial.printf("ON ");
number_key 52:320f910ca6ca 232 else debugSerial.printf("%d ",receiveSuccessed1);
number_key 49:69a7235d837a 233
number_key 52:320f910ca6ca 234 if(receiveSuccessed2 == 1) debugSerial.printf("ON\r\n");
number_key 52:320f910ca6ca 235 else debugSerial.printf("%d\r\n",receiveSuccessed2);
number_key 49:69a7235d837a 236 }
number_key 49:69a7235d837a 237
number_key 49:69a7235d837a 238 void Bot::checkDegree()
number_key 49:69a7235d837a 239 {
number_key 49:69a7235d837a 240 debugSerial.printf("%f %f\r\n",plane.getRawDegree(), plane.getCalculationResult());
number_key 49:69a7235d837a 241 }
number_key 52:320f910ca6ca 242
number_key 52:320f910ca6ca 243 void Bot::checkPacket()
number_key 52:320f910ca6ca 244 {
number_key 52:320f910ca6ca 245 char buf=0;
number_key 52:320f910ca6ca 246 while(1) {
number_key 52:320f910ca6ca 247 buf = pad1.getc();
number_key 52:320f910ca6ca 248 debugSerial.printf("%d %c\r\n",buf,buf);
number_key 52:320f910ca6ca 249 }
number_key 52:320f910ca6ca 250 }
number_key 52:320f910ca6ca 251
number_key 52:320f910ca6ca 252 void Bot::checkReceiveData()
number_key 52:320f910ca6ca 253 {
number_key 52:320f910ca6ca 254 debugSerial.printf("%f %f %f %f\r\n",pad1.getStick(0),pad1.getStick(1),pad1.getStick(2),pad1.getStick(3));
number_key 52:320f910ca6ca 255 debugSerial.printf("%f %f %f %f\r\n",pad2.getStick(0),pad2.getStick(1),pad2.getStick(2),pad2.getStick(3));
number_key 52:320f910ca6ca 256 }