NagaokaRoboticsClub_mbedTeam / Mbed OS NHK2017_octopus2_drive

タコ　駆動側　

Dependencies:   2017NHKpin_config FEP R1307 PID ikarashiMDC omni_wheel

Fork of NHK2017_octopus2 by NagaokaRoboticsClub_mbedTeam

bot/bot.cpp@56:e9a5cfd23833, 2017-11-28 (annotated)

Committer:
number_key
Date:
Tue Nov 28 17:54:16 2017 +0900
Revision:
56:e9a5cfd23833
Parent:
55:52b2cb45f21f 
add drive5

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 54:7176e6a08600 4 pad(XBee1TX, XBee1RX, ADDR1),
number_key 50:3a7c858aa0f9 5 RS485(MDTX, MDRX, 115200),
uchitake 9:39be1525dfe0 6 RS485Controller(PWM1),
UCHITAKE 15:9aa11febe517 7 powerSwitch(MDstop),
uchitake 9:39be1525dfe0 8 quadOmni(&RS485Controller, &RS485),
takeuchi 47:43f55ff8916b 9 plane(),
takeuchi 47:43f55ff8916b 10 axis(),
number_key 54:7176e6a08600 11 receiveSuccessed(0),
uchitake 28:676330f1d186 12 frontDegree(0),
number_key 55:52b2cb45f21f 13 led(LED1),
uchitake 9:39be1525dfe0 14 debugSerial(USBTX, USBRX, 115200)
uchitake 1:845af5425eec 15 {
takeuchi 47:43f55ff8916b 16 debugSerial.printf("OK\r\n");
uchitake 9:39be1525dfe0 17
uchitake 9:39be1525dfe0 18 quadOmni.moveXY(0, 0, 0);
UCHITAKE 15:9aa11febe517 19 powerSwitch = true;
uchitake 1:845af5425eec 20 }
uchitake 1:845af5425eec 21
number_key 54:7176e6a08600 22 void Bot::confirmPad()
uchitake 1:845af5425eec 23 {
number_key 54:7176e6a08600 24 receiveSuccessed = pad.receiveState();
number_key 54:7176e6a08600 25 if(!pad.getButton2(0) && !pad.getButton2(1)) {
uchitake 16:50651ff960b9 26 powerSwitch = 0;
uchitake 16:50651ff960b9 27 }
number_key 54:7176e6a08600 28 if(!pad.getButton2(2) && !pad.getButton2(3)) {
number_key 50:3a7c858aa0f9 29 powerSwitch = 1;
number_key 50:3a7c858aa0f9 30 }
number_key 54:7176e6a08600 31 if(!receiveSuccessed) {
uchitake 9:39be1525dfe0 32 quadOmni.moveXY(0, 0, 0);
number_key 49:69a7235d837a 33 }
number_key 49:69a7235d837a 34 }
number_key 49:69a7235d837a 35
uchitake 1:845af5425eec 36 void Bot::controllDrive()
uchitake 1:845af5425eec 37 {
number_key 54:7176e6a08600 38 if(receiveSuccessed) {
number_key 49:69a7235d837a 39 float moment = 0;
number_key 49:69a7235d837a 40 //debugSerial.printf("%d\n\r", plane.getRawDegree());
number_key 54:7176e6a08600 41 /*if(pad.getNorm(0) > 0.5) {
number_key 54:7176e6a08600 42 plane.setPoint((pad.getRadian(0) - M_PI / 2) * (180.0 / M_PI));
takeuchi 32:b619c7787dc3 43 plane.confirm();
number_key 49:69a7235d837a 44 }*/
number_key 49:69a7235d837a 45 plane.confirm();
number_key 54:7176e6a08600 46 if(pad.getStick(0)) {
number_key 54:7176e6a08600 47 moment = -pad.getStick(0) / 2.0;
number_key 49:69a7235d837a 48 plane.resetOffset();
number_key 49:69a7235d837a 49 } else {
number_key 49:69a7235d837a 50 moment = -plane.getCalculationResult();
uchitake 3:369d9ee17e84 51 }
number_key 49:69a7235d837a 52 debugSerial.printf("%f\r\n",moment);
uchitake 9:39be1525dfe0 53 quadOmni.moveXY(
number_key 54:7176e6a08600 54 pad.getStick(2),
number_key 54:7176e6a08600 55 -pad.getStick(3),
takeuchi 37:6b6616008e78 56 0,
takeuchi 37:6b6616008e78 57 0,
number_key 49:69a7235d837a 58 moment
takeuchi 37:6b6616008e78 59 );
uchitake 3:369d9ee17e84 60 } else {
uchitake 9:39be1525dfe0 61 quadOmni.moveXY(0, 0, 0);
uchitake 3:369d9ee17e84 62 }
uchitake 3:369d9ee17e84 63 }
uchitake 3:369d9ee17e84 64
uchitake 3:369d9ee17e84 65 void Bot::controllDrive2()
uchitake 3:369d9ee17e84 66 {
number_key 50:3a7c858aa0f9 67 plane.confirm();
number_key 50:3a7c858aa0f9 68 axis.confirm();
number_key 50:3a7c858aa0f9 69 float moment = 0;
number_key 50:3a7c858aa0f9 70 float norm = 0;
number_key 54:7176e6a08600 71 static float beforestick = pad.getStick(0);
uchitake 6:fe9767a50891 72
number_key 54:7176e6a08600 73 if(!pad.getButton2(5)) {
number_key 50:3a7c858aa0f9 74 axis.resetOffset();
number_key 52:320f910ca6ca 75 //debugSerial.printf("Force RESeT\n\r");
number_key 50:3a7c858aa0f9 76 }
uchitake 3:369d9ee17e84 77
number_key 54:7176e6a08600 78 if((beforestick >= 0.2 && pad.getStick(0) < 0.2) || (beforestick <= -0.2 && pad.getStick(0) > -0.2)) {
number_key 50:3a7c858aa0f9 79 plane.setPoint(0.0);
number_key 50:3a7c858aa0f9 80 plane.resetOffset();
number_key 50:3a7c858aa0f9 81 }
number_key 54:7176e6a08600 82 /*if(!pad.getButton2(1)&&pad.getButton2(3)) {
number_key 50:3a7c858aa0f9 83 plane.setPoint((M_PI / 4)*(180.0 / M_PI));
number_key 54:7176e6a08600 84 } else if(pad.getButton2(1)&&!pad.getButton2(3)){
number_key 50:3a7c858aa0f9 85 plane.setPoint(-(M_PI / 4)*(180.0 / M_PI));
number_key 50:3a7c858aa0f9 86 } else {
number_key 50:3a7c858aa0f9 87 plane.setPoint(0.0);
number_key 50:3a7c858aa0f9 88 }*/
number_key 54:7176e6a08600 89 if(pad.getStick(0) > 0.2 || pad.getStick(0) < -0.2) {
number_key 55:52b2cb45f21f 90 moment = pad.getStick(0);
number_key 50:3a7c858aa0f9 91 } else {
number_key 50:3a7c858aa0f9 92 moment = plane.getCalculationResult();
number_key 50:3a7c858aa0f9 93
number_key 50:3a7c858aa0f9 94 }
number_key 55:52b2cb45f21f 95 // if(pad.getButton1(0)) {
number_key 54:7176e6a08600 96 norm = pad.getNorm(1);
number_key 55:52b2cb45f21f 97 // } else {
number_key 55:52b2cb45f21f 98 // norm = pad.getNorm(1) / 2.0;
number_key 55:52b2cb45f21f 99 // }
number_key 54:7176e6a08600 100 if(receiveSuccessed) {
number_key 50:3a7c858aa0f9 101 quadOmni.moveCircular(
number_key 50:3a7c858aa0f9 102 norm,
number_key 54:7176e6a08600 103 pad.getRadian(1) - axis.getCurrentDegree() /1.0 * (M_PI / 180.0) + M_PI,
number_key 50:3a7c858aa0f9 104 0.0,
number_key 50:3a7c858aa0f9 105 0.0,
number_key 50:3a7c858aa0f9 106 -moment
number_key 50:3a7c858aa0f9 107 );
number_key 50:3a7c858aa0f9 108 } else {
number_key 50:3a7c858aa0f9 109 quadOmni.moveXY(0, 0, 0);
number_key 50:3a7c858aa0f9 110 }
number_key 54:7176e6a08600 111 beforestick = pad.getStick(0);
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;
number_key 54:7176e6a08600 119 if(receiveSuccessed) {
number_key 54:7176e6a08600 120 if(rollR && !pad.getButton2(2)) {
uchitake 28:676330f1d186 121 frontDegree += ADJUST_DEGREE;
uchitake 28:676330f1d186 122 }
number_key 54:7176e6a08600 123 rollR = pad.getButton2(2);
uchitake 28:676330f1d186 124
number_key 54:7176e6a08600 125 if(rollL && !pad.getButton2(0)) {
uchitake 28:676330f1d186 126 frontDegree -= ADJUST_DEGREE;
uchitake 28:676330f1d186 127 }
number_key 54:7176e6a08600 128 rollL = pad.getButton2(0);
uchitake 17:79fa65706f92 129
number_key 54:7176e6a08600 130 if(!pad.getButton2(4)) {
uchitake 17:79fa65706f92 131 mode = 1;
uchitake 17:79fa65706f92 132 }
uchitake 17:79fa65706f92 133
number_key 54:7176e6a08600 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(
number_key 54:7176e6a08600 143 pad.getStick(0),
number_key 54:7176e6a08600 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(
number_key 54:7176e6a08600 155 -pad.getStick(1),
number_key 54:7176e6a08600 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 {
number_key 54:7176e6a08600 169 if(receiveSuccessed) {
takeuchi 41:ae6f844facb1 170 quadOmni.moveXY(
number_key 54:7176e6a08600 171 pad.getStick(2),
number_key 55:52b2cb45f21f 172 -pad.getStick(3),
takeuchi 41:ae6f844facb1 173 0.0,
takeuchi 41:ae6f844facb1 174 0.0,
number_key 55:52b2cb45f21f 175 -pad.getStick(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
number_key 55:52b2cb45f21f 182 void Bot::controllDrive5()
number_key 55:52b2cb45f21f 183 {
number_key 55:52b2cb45f21f 184 plane.confirm();
number_key 55:52b2cb45f21f 185 axis.confirm();
number_key 55:52b2cb45f21f 186 float moment = 0;
number_key 55:52b2cb45f21f 187 float norm = 0;
number_key 55:52b2cb45f21f 188 static float beforestick = pad.getStick(0);
number_key 55:52b2cb45f21f 189 static int mode = 0;
number_key 55:52b2cb45f21f 190
number_key 55:52b2cb45f21f 191 led = mode;
number_key 55:52b2cb45f21f 192 if(!pad.getButton2(5)) {
number_key 55:52b2cb45f21f 193 axis.resetOffset();
number_key 55:52b2cb45f21f 194 }
number_key 55:52b2cb45f21f 195
number_key 55:52b2cb45f21f 196 if((beforestick >= 0.2 && pad.getStick(0) < 0.2) || (beforestick <= -0.2 && pad.getStick(0) > -0.2)) {
number_key 55:52b2cb45f21f 197 plane.setPoint(0.0);
number_key 55:52b2cb45f21f 198 plane.resetOffset();
number_key 55:52b2cb45f21f 199 }
number_key 55:52b2cb45f21f 200 if(pad.getStick(0) > 0.2 || pad.getStick(0) < -0.2) {
number_key 55:52b2cb45f21f 201 moment = pad.getStick(0);
number_key 55:52b2cb45f21f 202 } else {
number_key 55:52b2cb45f21f 203 moment = plane.getCalculationResult();
number_key 55:52b2cb45f21f 204 }
number_key 55:52b2cb45f21f 205 norm = pad.getNorm(1);
number_key 55:52b2cb45f21f 206
number_key 55:52b2cb45f21f 207 if(receiveSuccessed) {
number_key 55:52b2cb45f21f 208 if(!pad.getButton2(0) && !pad.getButton2(1)) {
number_key 55:52b2cb45f21f 209 mode = 0;
number_key 55:52b2cb45f21f 210 }
number_key 55:52b2cb45f21f 211 if(!pad.getButton2(2) && !pad.getButton2(3)) {
number_key 55:52b2cb45f21f 212 mode = 1;
number_key 55:52b2cb45f21f 213 }
number_key 55:52b2cb45f21f 214 if(mode){
number_key 55:52b2cb45f21f 215 quadOmni.moveCircular(
number_key 55:52b2cb45f21f 216 norm,
number_key 55:52b2cb45f21f 217 pad.getRadian(1) - axis.getCurrentDegree() /1.0 * (M_PI / 180.0) + M_PI,
number_key 55:52b2cb45f21f 218 0.0,
number_key 55:52b2cb45f21f 219 0.0,
number_key 55:52b2cb45f21f 220 -moment
number_key 55:52b2cb45f21f 221 );
number_key 55:52b2cb45f21f 222 } else {
number_key 55:52b2cb45f21f 223 quadOmni.moveXY(
number_key 55:52b2cb45f21f 224 pad.getStick(2),
number_key 55:52b2cb45f21f 225 -pad.getStick(3),
number_key 55:52b2cb45f21f 226 0.0,
number_key 55:52b2cb45f21f 227 0.0,
number_key 55:52b2cb45f21f 228 -pad.getStick(0)
number_key 55:52b2cb45f21f 229 );
number_key 55:52b2cb45f21f 230 }
number_key 55:52b2cb45f21f 231 } else {
number_key 55:52b2cb45f21f 232 quadOmni.moveXY(0, 0, 0);
number_key 55:52b2cb45f21f 233 }
number_key 55:52b2cb45f21f 234 beforestick = pad.getStick(0);
number_key 55:52b2cb45f21f 235 }
number_key 55:52b2cb45f21f 236
number_key 55:52b2cb45f21f 237
uchitake 1:845af5425eec 238 void Bot::calibrate()
uchitake 1:845af5425eec 239 {
uchitake 3:369d9ee17e84 240 }
number_key 49:69a7235d837a 241
number_key 49:69a7235d837a 242 void Bot::checkConnection()
number_key 49:69a7235d837a 243 {
number_key 54:7176e6a08600 244 if(receiveSuccessed == 1) debugSerial.printf("ON \r\n");
number_key 54:7176e6a08600 245 else debugSerial.printf("OFF \r\n");
number_key 49:69a7235d837a 246 }
number_key 49:69a7235d837a 247
number_key 49:69a7235d837a 248 void Bot::checkDegree()
number_key 49:69a7235d837a 249 {
number_key 49:69a7235d837a 250 debugSerial.printf("%f %f\r\n",plane.getRawDegree(), plane.getCalculationResult());
number_key 49:69a7235d837a 251 }
number_key 52:320f910ca6ca 252
number_key 52:320f910ca6ca 253
number_key 52:320f910ca6ca 254 void Bot::checkReceiveData()
number_key 52:320f910ca6ca 255 {
number_key 54:7176e6a08600 256 debugSerial.printf("%f %f %f %f\r\n",pad.getStick(0),pad.getStick(1),pad.getStick(2),pad.getStick(3));
number_key 52:320f910ca6ca 257 }