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@19:34da005ea4ea, 2017-09-28 (annotated)

Committer:
uchitake
Date:
Thu Sep 28 20:37:09 2017 +0900
Branch:
develop1
Revision:
19:34da005ea4ea
Parent:
17:79fa65706f92
Child:
23:797d25f3df5e
Child:
24:593910fa1397
works all unit

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 6:fe9767a50891 4 PIDC(),
UCHITAKE 15:9aa11febe517 5 pad(XBee2TX, XBee2RX, ADDR),
uchitake 9:39be1525dfe0 6 RS485(MDTX, MDRX, 38400),
uchitake 9:39be1525dfe0 7 RS485Controller(PWM1),
UCHITAKE 15:9aa11febe517 8 powerSwitch(MDstop),
uchitake 9:39be1525dfe0 9 quadOmni(&RS485Controller, &RS485),
uchitake 9:39be1525dfe0 10 slider(&RS485Controller, &RS485),
uchitake 9:39be1525dfe0 11 armMotor({
UCHITAKE 11:a8385ca0a275 12 ikarashiMDC({&RS485Controller, 1, 1, SM, &RS485}),
UCHITAKE 11:a8385ca0a275 13 ikarashiMDC({&RS485Controller, 1, 2, SM, &RS485}),
UCHITAKE 11:a8385ca0a275 14 ikarashiMDC({&RS485Controller, 1, 3, SM, &RS485})
uchitake 9:39be1525dfe0 15 }),
uchitake 9:39be1525dfe0 16 receiveSuccessed(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) {
uchitake 17:79fa65706f92 53 debugSerial.printf("%d\n\r", PIDC::getRawDegree());
uchitake 14:1fadf7d2f583 54 led[1] = !led[1];
uchitake 3:369d9ee17e84 55 if(pad.getNorm(1) > 0.5) {
uchitake 17:79fa65706f92 56 PIDC::PID::setSetPoint((pad.getRadian(1) - M_PI / 2) * (180.0 / M_PI));
uchitake 17:79fa65706f92 57 PIDC::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 15:9aa11febe517 62 - pad.getStick(2) / 3.0 // PIDC::calculationResult
uchitake 6:fe9767a50891 63 );
uchitake 3:369d9ee17e84 64 } else {
uchitake 9:39be1525dfe0 65 quadOmni.moveXY(0, 0, 0);
uchitake 3:369d9ee17e84 66 }
uchitake 3:369d9ee17e84 67 }
uchitake 3:369d9ee17e84 68
uchitake 3:369d9ee17e84 69 void Bot::controllDrive2()
uchitake 3:369d9ee17e84 70 {
uchitake 4:1073deb368df 71 float moment = 0;
uchitake 3:369d9ee17e84 72 static float beforestick = pad.getStick(2);
uchitake 6:fe9767a50891 73
uchitake 5:16ea97725085 74 if(!pad.getButton2(2)) {
uchitake 6:fe9767a50891 75 PIDC::resetPlaneOffset();
uchitake 5:16ea97725085 76 }
uchitake 3:369d9ee17e84 77
uchitake 4:1073deb368df 78 if((beforestick >= 0.5 && pad.getStick(2) < 0.5) || (beforestick <= -0.5 && pad.getStick(2) > -0.5)) {
uchitake 5:16ea97725085 79 PIDC::PID::setSetPoint(0.0);
uchitake 6:fe9767a50891 80 PIDC::resetAxisOffset();
uchitake 3:369d9ee17e84 81 }
uchitake 3:369d9ee17e84 82
uchitake 4:1073deb368df 83 if(pad.getStick(2) > 0.5 || pad.getStick(2) < -0.5) {
uchitake 5:16ea97725085 84 moment = pad.getStick(2) / 4.0;
uchitake 3:369d9ee17e84 85 PIDC::confirm();
uchitake 4:1073deb368df 86 }
uchitake 4:1073deb368df 87 if(fabs(pad.getStick(2)) < 0.5) {
uchitake 3:369d9ee17e84 88 PIDC::confirm();
uchitake 6:fe9767a50891 89 moment = PIDC::calculationResult;
uchitake 3:369d9ee17e84 90 }
uchitake 3:369d9ee17e84 91
uchitake 6:fe9767a50891 92 if(receiveSuccessed) {
uchitake 14:1fadf7d2f583 93 led[1] = !led[1];
uchitake 9:39be1525dfe0 94 quadOmni.moveCircular(
uchitake 6:fe9767a50891 95 pad.getNorm(0) / 2.0,
uchitake 6:fe9767a50891 96 pad.getRadian(0) - PIDC::planeCurrentDegree / 10.0 * (M_PI / 180.0) + M_PI,
uchitake 6:fe9767a50891 97 moment
uchitake 6:fe9767a50891 98 );
uchitake 3:369d9ee17e84 99 } else {
uchitake 9:39be1525dfe0 100 quadOmni.moveXY(0, 0, 0);
uchitake 3:369d9ee17e84 101 }
uchitake 3:369d9ee17e84 102 beforestick = pad.getStick(2);
uchitake 1:845af5425eec 103 }
uchitake 1:845af5425eec 104
uchitake 17:79fa65706f92 105 void Bot::controllDrive3()
uchitake 17:79fa65706f92 106 {
uchitake 17:79fa65706f92 107 static int mode = 1;
uchitake 17:79fa65706f92 108 if(receiveSuccessed) {
uchitake 17:79fa65706f92 109 debugSerial.printf("%d\n\r", PIDC::getCurrentDegree());
uchitake 17:79fa65706f92 110
uchitake 17:79fa65706f92 111 if(!pad.getButton2(4)) {
uchitake 17:79fa65706f92 112 mode = 1;
uchitake 17:79fa65706f92 113 }
uchitake 17:79fa65706f92 114
uchitake 17:79fa65706f92 115 if(!pad.getButton2(5)) {
uchitake 17:79fa65706f92 116 mode = 2;
uchitake 17:79fa65706f92 117 }
uchitake 17:79fa65706f92 118
uchitake 17:79fa65706f92 119 if(mode == 1) {
uchitake 17:79fa65706f92 120 PIDC::PID::setSetPoint(0.0);
uchitake 17:79fa65706f92 121 PIDC::confirm();
uchitake 17:79fa65706f92 122
uchitake 17:79fa65706f92 123 quadOmni.moveXY(
uchitake 17:79fa65706f92 124 pad.getStick(0),
uchitake 17:79fa65706f92 125 -pad.getStick(1),
uchitake 17:79fa65706f92 126 -PIDC::calculationResult
uchitake 17:79fa65706f92 127 );
uchitake 17:79fa65706f92 128 }
uchitake 17:79fa65706f92 129 if(mode == 2) {
uchitake 17:79fa65706f92 130 PIDC::PID::setSetPoint(90.0);
uchitake 17:79fa65706f92 131 PIDC::confirm();
uchitake 17:79fa65706f92 132
uchitake 17:79fa65706f92 133 quadOmni.moveXY(
uchitake 17:79fa65706f92 134 -pad.getStick(1),
uchitake 19:34da005ea4ea 135 -pad.getStick(0),
uchitake 17:79fa65706f92 136 -PIDC::calculationResult
uchitake 17:79fa65706f92 137 );
uchitake 17:79fa65706f92 138 }
uchitake 17:79fa65706f92 139 } else {
uchitake 17:79fa65706f92 140 quadOmni.moveXY(0, 0, 0);
uchitake 17:79fa65706f92 141 }
uchitake 17:79fa65706f92 142 }
uchitake 17:79fa65706f92 143
uchitake 1:845af5425eec 144 void Bot::controllMech()
uchitake 1:845af5425eec 145 {
uchitake 6:fe9767a50891 146 if(receiveSuccessed) {
uchitake 16:50651ff960b9 147 if(!pad.getButton1(6)) {
uchitake 16:50651ff960b9 148 armMotor[SWORD].setSpeed(0.6);
uchitake 16:50651ff960b9 149 }
uchitake 16:50651ff960b9 150 if(pad.getButton1(6)) armMotor[SWORD].setSpeed(0.0);
uchitake 1:845af5425eec 151
uchitake 19:34da005ea4ea 152 slider.slide(pad.getStick(3));
UCHITAKE 15:9aa11febe517 153
uchitake 16:50651ff960b9 154 if(!pad.getButton2(1)) {
UCHITAKE 15:9aa11febe517 155 armMotor[DESTROY].setSpeed(-1.0);
UCHITAKE 15:9aa11febe517 156 } else {
UCHITAKE 15:9aa11febe517 157 armMotor[DESTROY].setSpeed(0.0);
UCHITAKE 15:9aa11febe517 158 }
uchitake 3:369d9ee17e84 159 } else {
uchitake 9:39be1525dfe0 160 slider.slide(0);
uchitake 9:39be1525dfe0 161 for(int i = 0; i < 3; i++) {
uchitake 9:39be1525dfe0 162 armMotor[i].setSpeed(0);
uchitake 9:39be1525dfe0 163 }
uchitake 3:369d9ee17e84 164 }
uchitake 1:845af5425eec 165 }
uchitake 1:845af5425eec 166
uchitake 1:845af5425eec 167
uchitake 1:845af5425eec 168 void Bot::calibrate()
uchitake 1:845af5425eec 169 {
uchitake 6:fe9767a50891 170 if(receiveSuccessed &&
uchitake 6:fe9767a50891 171 !pad.getButton2(0) &&
uchitake 6:fe9767a50891 172 !pad.getButton2(1)
uchitake 6:fe9767a50891 173 ) {
uchitake 19:34da005ea4ea 174 t.start();
uchitake 19:34da005ea4ea 175 t.reset();
uchitake 1:845af5425eec 176 PIDC::calibration(HMC6352_ENTER_CALIB);
uchitake 19:34da005ea4ea 177 while(t.read() < 5.0) {
uchitake 19:34da005ea4ea 178 quadOmni.moveXY(0, 0, 0.4);
uchitake 19:34da005ea4ea 179 slider.slide(0);
uchitake 19:34da005ea4ea 180 for(int i = 0; i < 3; i++) {
uchitake 19:34da005ea4ea 181 armMotor[i].setSpeed(0);
uchitake 19:34da005ea4ea 182 }
uchitake 19:34da005ea4ea 183 }
uchitake 19:34da005ea4ea 184 t.stop();
uchitake 9:39be1525dfe0 185 quadOmni.moveXY(0, 0, 0);
uchitake 1:845af5425eec 186 PIDC::calibration(HMC6352_EXIT_CALIB);
uchitake 1:845af5425eec 187 }
uchitake 3:369d9ee17e84 188 }