NagaokaRoboticsClub_mbedTeam / Mbed OS NHK2017_octopus2

Octopus!!

Dependencies:   2017NHKpin_config FEP HMC6352 PID QEI R1307 ikarashiMDC omni_wheel

Fork of KANIv3 by NagaokaRoboticsClub_mbedTeam

bot/bot.cpp@47:43f55ff8916b, 2017-11-06 (annotated)

Committer:
takeuchi
Date:
Mon Nov 06 18:27:03 2017 +0900
Revision:
47:43f55ff8916b
Parent:
46:9b714d4acdac
Child:
49:69a7235d837a
R1370ver

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