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@35:4608938f67c5, 2017-10-07 (annotated)

Committer:
takeuchi
Date:
Sat Oct 07 19:13:33 2017 +0900
Revision:
35:4608938f67c5
Parent:
32:b619c7787dc3
Child:
36:c1398ea8f604
change files

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({
UCHITAKE 11:a8385ca0a275 11 ikarashiMDC({&RS485Controller, 1, 1, SM, &RS485}),
UCHITAKE 11:a8385ca0a275 12 ikarashiMDC({&RS485Controller, 1, 2, SM, &RS485}),
UCHITAKE 11:a8385ca0a275 13 ikarashiMDC({&RS485Controller, 1, 3, SM, &RS485})
uchitake 9:39be1525dfe0 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(
UCHITAKE 15:9aa11febe517 60 pad.getStick(0),
UCHITAKE 15:9aa11febe517 61 -pad.getStick(1),
uchitake 24:593910fa1397 62 0,
uchitake 24:593910fa1397 63 0,
UCHITAKE 15:9aa11febe517 64 - pad.getStick(2) / 3.0 // PIDC::calculationResult
uchitake 6:fe9767a50891 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;
uchitake 3:369d9ee17e84 76 static float beforestick = pad.getStick(2);
uchitake 6:fe9767a50891 77
takeuchi 35:4608938f67c5 78 if(!pad.getButton1(6)) {
takeuchi 32:b619c7787dc3 79 axis.resetOffset();
takeuchi 35:4608938f67c5 80 debugSerial.printf("Force RESeT\n\r");
uchitake 5:16ea97725085 81 }
uchitake 3:369d9ee17e84 82
uchitake 4:1073deb368df 83 if((beforestick >= 0.5 && pad.getStick(2) < 0.5) || (beforestick <= -0.5 && pad.getStick(2) > -0.5)) {
takeuchi 32:b619c7787dc3 84 plane.setSetPoint(0.0);
takeuchi 32:b619c7787dc3 85 plane.resetOffset();
uchitake 3:369d9ee17e84 86 }
uchitake 3:369d9ee17e84 87
uchitake 4:1073deb368df 88 if(pad.getStick(2) > 0.5 || pad.getStick(2) < -0.5) {
uchitake 29:41f6fc4c8962 89 moment = pad.getStick(2) / 2.0;
uchitake 29:41f6fc4c8962 90 } else {
takeuchi 32:b619c7787dc3 91 moment = plane.getCalculationResult();
uchitake 3:369d9ee17e84 92 }
uchitake 3:369d9ee17e84 93
uchitake 6:fe9767a50891 94 if(receiveSuccessed) {
uchitake 14:1fadf7d2f583 95 led[1] = !led[1];
uchitake 9:39be1525dfe0 96 quadOmni.moveCircular(
uchitake 29:41f6fc4c8962 97 pad.getNorm(0),
takeuchi 32:b619c7787dc3 98 pad.getRadian(0) - axis.getCurrentDegree() / 10.0 * (M_PI / 180.0) + M_PI,
uchitake 29:41f6fc4c8962 99 0.5,
uchitake 29:41f6fc4c8962 100 0.5,
uchitake 29:41f6fc4c8962 101 -moment
uchitake 6:fe9767a50891 102 );
uchitake 3:369d9ee17e84 103 } else {
uchitake 9:39be1525dfe0 104 quadOmni.moveXY(0, 0, 0);
uchitake 3:369d9ee17e84 105 }
uchitake 3:369d9ee17e84 106 beforestick = pad.getStick(2);
uchitake 1:845af5425eec 107 }
uchitake 1:845af5425eec 108
uchitake 17:79fa65706f92 109 void Bot::controllDrive3()
uchitake 17:79fa65706f92 110 {
uchitake 28:676330f1d186 111 static int rollR = 0;
uchitake 28:676330f1d186 112 static int rollL = 0;
uchitake 17:79fa65706f92 113 static int mode = 1;
uchitake 17:79fa65706f92 114 if(receiveSuccessed) {
uchitake 28:676330f1d186 115 if(rollR && !pad.getButton2(2)) {
uchitake 28:676330f1d186 116 frontDegree += ADJUST_DEGREE;
uchitake 28:676330f1d186 117 }
uchitake 28:676330f1d186 118 rollR = pad.getButton2(2);
uchitake 28:676330f1d186 119
uchitake 28:676330f1d186 120 if(rollL && !pad.getButton2(0)) {
uchitake 28:676330f1d186 121 frontDegree -= ADJUST_DEGREE;
uchitake 28:676330f1d186 122 }
uchitake 28:676330f1d186 123 rollL = pad.getButton2(0);
uchitake 17:79fa65706f92 124
uchitake 17:79fa65706f92 125 if(!pad.getButton2(4)) {
uchitake 17:79fa65706f92 126 mode = 1;
uchitake 17:79fa65706f92 127 }
uchitake 17:79fa65706f92 128
uchitake 17:79fa65706f92 129 if(!pad.getButton2(5)) {
uchitake 17:79fa65706f92 130 mode = 2;
uchitake 17:79fa65706f92 131 }
uchitake 17:79fa65706f92 132
uchitake 17:79fa65706f92 133 if(mode == 1) {
takeuchi 32:b619c7787dc3 134 plane.setPoint(frontDegree);
takeuchi 32:b619c7787dc3 135 plane.confirm();
uchitake 17:79fa65706f92 136
uchitake 17:79fa65706f92 137 quadOmni.moveXY(
uchitake 17:79fa65706f92 138 pad.getStick(0),
uchitake 17:79fa65706f92 139 -pad.getStick(1),
takeuchi 27:47c6eee26e76 140 0.5,
takeuchi 27:47c6eee26e76 141 0.5,
takeuchi 32:b619c7787dc3 142 -plane.getCalculationResult()
uchitake 17:79fa65706f92 143 );
uchitake 17:79fa65706f92 144 }
uchitake 17:79fa65706f92 145 if(mode == 2) {
takeuchi 32:b619c7787dc3 146 plane.setPoint(90.0 + frontDegree);
takeuchi 32:b619c7787dc3 147 plane.confirm();
uchitake 17:79fa65706f92 148
uchitake 17:79fa65706f92 149 quadOmni.moveXY(
uchitake 17:79fa65706f92 150 -pad.getStick(1),
uchitake 19:34da005ea4ea 151 -pad.getStick(0),
takeuchi 27:47c6eee26e76 152 0.5,
takeuchi 27:47c6eee26e76 153 0.5,
takeuchi 32:b619c7787dc3 154 -plane.getCalculationResult()
uchitake 17:79fa65706f92 155 );
uchitake 17:79fa65706f92 156 }
uchitake 17:79fa65706f92 157 } else {
uchitake 17:79fa65706f92 158 quadOmni.moveXY(0, 0, 0);
uchitake 17:79fa65706f92 159 }
uchitake 17:79fa65706f92 160 }
uchitake 17:79fa65706f92 161
uchitake 1:845af5425eec 162 void Bot::controllMech()
uchitake 1:845af5425eec 163 {
uchitake 6:fe9767a50891 164 if(receiveSuccessed) {
takeuchi 35:4608938f67c5 165 if(!pad.getButton1(0)) {
takeuchi 35:4608938f67c5 166 debugSerial.printf("FUKUDA\n\r");
uchitake 16:50651ff960b9 167 armMotor[SWORD].setSpeed(0.6);
uchitake 16:50651ff960b9 168 }
takeuchi 35:4608938f67c5 169 if(pad.getButton1(0)) armMotor[SWORD].setSpeed(0.0);
uchitake 1:845af5425eec 170
uchitake 19:34da005ea4ea 171 slider.slide(pad.getStick(3));
UCHITAKE 15:9aa11febe517 172
uchitake 16:50651ff960b9 173 if(!pad.getButton2(1)) {
takeuchi 35:4608938f67c5 174 debugSerial.printf("DESTROYYY\n\r");
UCHITAKE 15:9aa11febe517 175 armMotor[DESTROY].setSpeed(-1.0);
UCHITAKE 15:9aa11febe517 176 } else {
UCHITAKE 15:9aa11febe517 177 armMotor[DESTROY].setSpeed(0.0);
UCHITAKE 15:9aa11febe517 178 }
uchitake 3:369d9ee17e84 179 } else {
uchitake 9:39be1525dfe0 180 slider.slide(0);
uchitake 9:39be1525dfe0 181 for(int i = 0; i < 3; i++) {
uchitake 9:39be1525dfe0 182 armMotor[i].setSpeed(0);
uchitake 9:39be1525dfe0 183 }
uchitake 3:369d9ee17e84 184 }
uchitake 1:845af5425eec 185 }
uchitake 1:845af5425eec 186
uchitake 1:845af5425eec 187
uchitake 1:845af5425eec 188 void Bot::calibrate()
uchitake 1:845af5425eec 189 {
uchitake 6:fe9767a50891 190 if(receiveSuccessed &&
uchitake 6:fe9767a50891 191 !pad.getButton2(0) &&
uchitake 6:fe9767a50891 192 !pad.getButton2(1)
uchitake 6:fe9767a50891 193 ) {
uchitake 19:34da005ea4ea 194 t.start();
uchitake 19:34da005ea4ea 195 t.reset();
takeuchi 32:b619c7787dc3 196 plane.calibration(HMC6352_ENTER_CALIB);
uchitake 19:34da005ea4ea 197 while(t.read() < 5.0) {
uchitake 19:34da005ea4ea 198 quadOmni.moveXY(0, 0, 0.4);
uchitake 19:34da005ea4ea 199 slider.slide(0);
uchitake 19:34da005ea4ea 200 for(int i = 0; i < 3; i++) {
uchitake 19:34da005ea4ea 201 armMotor[i].setSpeed(0);
uchitake 19:34da005ea4ea 202 }
uchitake 19:34da005ea4ea 203 }
uchitake 19:34da005ea4ea 204 t.stop();
uchitake 9:39be1525dfe0 205 quadOmni.moveXY(0, 0, 0);
takeuchi 32:b619c7787dc3 206 plane.calibration(HMC6352_EXIT_CALIB);
uchitake 1:845af5425eec 207 }
uchitake 3:369d9ee17e84 208 }