NagaokaRoboticsClub_mbedTeam / Mbed OS NHK2017_octopus2_arms

タコ　腕

Dependencies:   2017NHKpin_config FEP ikarashiMDC

Fork of NHK2017_octopus2 by NagaokaRoboticsClub_mbedTeam

bot/bot.cpp@51:64534e908d5c, 2017-11-13 (annotated)

Committer:
number_key
Date:
Mon Nov 13 17:34:01 2017 +0900
Revision:
51:64534e908d5c
Parent:
50:3a7c858aa0f9 
Backout changeset 3a7c858aa0f973d081ff06fd37568bbd511ebbf8

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