NagaokaRoboticsClub_mbedTeam / Mbed 2 deprecated NHK2017_octopus

NHK2017 octopus robot

Dependencies:   2017NHKpin_config mbed FEP ikarashiMDC PID jy901 omni HMC6352 omni_wheel

Fork of KANI2017v2 by NagaokaRoboticsClub_mbedTeam

bot/bot.cpp@25:d199d621ecca, 2017-09-30 (annotated)

Committer:
number_key
Date:
Sat Sep 30 17:59:10 2017 +0900
Revision:
25:d199d621ecca
Parent:
24:9a4ca5442717
Child:
26:7258d5ad0bff
save

Who changed what in which revision?

UserRevisionLine numberNew contents of line
UCHITAKE 0:757e9800c103 1 #include "bot.h"
UCHITAKE 0:757e9800c103 2
uchitake 18:6f52f4b1086a 3 Bot::Bot() :
number_key 22:682cc376da6f 4 PIDC(),
number_key 22:682cc376da6f 5 pad1(XBee1TX, XBee1RX, ADDR1),
number_key 25:d199d621ecca 6 pad2(XBee2TX, XBee2RX, ADDR2),
number_key 23:37bb9afe9fdc 7 RS485(MDTX, MDRX, 38400),
number_key 24:9a4ca5442717 8 RS485Controller(PWM2),
number_key 23:37bb9afe9fdc 9 powerSwitch(MDstop),
number_key 23:37bb9afe9fdc 10 quadOmni(&RS485Controller, &RS485),
number_key 23:37bb9afe9fdc 11 tentacle(&RS485Controller, &RS485),
number_key 23:37bb9afe9fdc 12 nishijo(&RS485Controller, &RS485),
number_key 23:37bb9afe9fdc 13 nishijoSword(&RS485Controller, &RS485),
number_key 23:37bb9afe9fdc 14 receiveSuccessed1(0),
number_key 25:d199d621ecca 15 receiveSuccessed2(0),
number_key 23:37bb9afe9fdc 16 led({DebugLED1, DebugLED2, DebugLED3, DebugLED4,
number_key 23:37bb9afe9fdc 17 DebugLED5, DebugLED6, DebugLED7, DebugLED8}),
number_key 24:9a4ca5442717 18 test(PWM1),
number_key 25:d199d621ecca 19 debugSerial(USBTX, USBRX, 230400)
UCHITAKE 0:757e9800c103 20 {
number_key 23:37bb9afe9fdc 21
number_key 24:9a4ca5442717 22 test = 0;
number_key 23:37bb9afe9fdc 23 quadOmni.moveXY(0, 0, 0);
number_key 23:37bb9afe9fdc 24
number_key 23:37bb9afe9fdc 25 powerSwitch = 1;
number_key 23:37bb9afe9fdc 26
number_key 23:37bb9afe9fdc 27 for(int i = 0; i < 8; i++) {
number_key 23:37bb9afe9fdc 28 led[i] = 1;
number_key 23:37bb9afe9fdc 29 wait(0.1);
number_key 23:37bb9afe9fdc 30 led[i] = 0;
number_key 23:37bb9afe9fdc 31 }
number_key 23:37bb9afe9fdc 32 for(int i = 0; i < 8; i++) {
number_key 23:37bb9afe9fdc 33 led[i] = 1;
number_key 23:37bb9afe9fdc 34 wait(0.1);
number_key 23:37bb9afe9fdc 35 led[i] = 0;
number_key 23:37bb9afe9fdc 36 }
UCHITAKE 1:269914e0aa07 37 }
UCHITAKE 1:269914e0aa07 38
UCHITAKE 1:269914e0aa07 39 void Bot::confirmAll()
UCHITAKE 1:269914e0aa07 40 {
number_key 23:37bb9afe9fdc 41 receiveSuccessed1 = pad1.receiveState();
number_key 25:d199d621ecca 42 receiveSuccessed2 = pad2.receiveState();
number_key 23:37bb9afe9fdc 43 if(!receiveSuccessed1) {
number_key 23:37bb9afe9fdc 44 quadOmni.moveXY(0, 0, 0);
number_key 23:37bb9afe9fdc 45 tentacle.stop();
number_key 22:682cc376da6f 46 }
UCHITAKE 1:269914e0aa07 47 }
UCHITAKE 1:269914e0aa07 48
UCHITAKE 1:269914e0aa07 49 void Bot::controllDrive()
UCHITAKE 1:269914e0aa07 50 {
number_key 25:d199d621ecca 51 test = !test;
number_key 23:37bb9afe9fdc 52 if(receiveSuccessed1) {
number_key 25:d199d621ecca 53 if(pad1.getNorm(0) > 0.5) {
number_key 25:d199d621ecca 54 PIDC::PID::setSetPoint((pad1.getRadian(0) - M_PI / 2) * (180.0 / M_PI));
number_key 22:682cc376da6f 55 PIDC::confirm();
number_key 22:682cc376da6f 56 }
number_key 24:9a4ca5442717 57 //PIDC::confirm();
number_key 23:37bb9afe9fdc 58 PIDC::renewAngle();
number_key 25:d199d621ecca 59 //test = !test;
number_key 23:37bb9afe9fdc 60 quadOmni.moveXY(
number_key 24:9a4ca5442717 61 pad1.getStick(0) * 0.8,
number_key 24:9a4ca5442717 62 -pad1.getStick(1) * 0.8,
number_key 25:d199d621ecca 63 pad1.getStick(2) * 0.4
number_key 24:9a4ca5442717 64 //PIDC::calculationResult
number_key 22:682cc376da6f 65 );
number_key 24:9a4ca5442717 66 /*quadOmni.moveCircular(
number_key 24:9a4ca5442717 67 0.2,
number_key 24:9a4ca5442717 68 0,
number_key 24:9a4ca5442717 69 0
number_key 24:9a4ca5442717 70 );*/
number_key 22:682cc376da6f 71 } else {
number_key 23:37bb9afe9fdc 72 quadOmni.moveXY(0, 0, 0);
number_key 22:682cc376da6f 73 }
number_key 22:682cc376da6f 74 }
number_key 22:682cc376da6f 75
number_key 22:682cc376da6f 76 void Bot::controllDrive2()
number_key 22:682cc376da6f 77 {
number_key 22:682cc376da6f 78 float moment = 0;
number_key 22:682cc376da6f 79 static float beforestick = pad1.getStick(2);
number_key 22:682cc376da6f 80
number_key 22:682cc376da6f 81 if(!pad1.getButton2(2)) {
number_key 22:682cc376da6f 82 PIDC::resetPlaneOffset();
number_key 22:682cc376da6f 83 }
number_key 22:682cc376da6f 84
number_key 22:682cc376da6f 85 if((beforestick >= 0.5 && pad1.getStick(2) < 0.5) || (beforestick <= -0.5 && pad1.getStick(2) > -0.5)) {
number_key 22:682cc376da6f 86 PIDC::PID::setSetPoint(0.0);
number_key 22:682cc376da6f 87 PIDC::resetAxisOffset();
number_key 22:682cc376da6f 88 }
number_key 22:682cc376da6f 89
number_key 22:682cc376da6f 90 if(pad1.getStick(2) > 0.5 || pad1.getStick(2) < -0.5) {
number_key 22:682cc376da6f 91 moment = pad1.getStick(2) / 4.0;
number_key 22:682cc376da6f 92 PIDC::confirm();
number_key 22:682cc376da6f 93 }
number_key 22:682cc376da6f 94 if(fabs(pad1.getStick(2)) < 0.5) {
number_key 22:682cc376da6f 95 PIDC::confirm();
number_key 22:682cc376da6f 96 moment = PIDC::calculationResult;
number_key 22:682cc376da6f 97 }
number_key 22:682cc376da6f 98
number_key 23:37bb9afe9fdc 99 if(receiveSuccessed1) {
number_key 23:37bb9afe9fdc 100 quadOmni.moveCircular(
number_key 22:682cc376da6f 101 pad1.getNorm(0) / 2.0,
number_key 22:682cc376da6f 102 pad1.getRadian(0) - PIDC::planeCurrentDegree / 10.0 * (M_PI / 180.0) + M_PI,
number_key 22:682cc376da6f 103 moment
number_key 22:682cc376da6f 104 );
number_key 22:682cc376da6f 105 } else {
number_key 23:37bb9afe9fdc 106 quadOmni.moveXY(0, 0, 0);
number_key 22:682cc376da6f 107 }
number_key 22:682cc376da6f 108 beforestick = pad1.getStick(2);
UCHITAKE 1:269914e0aa07 109 }
UCHITAKE 1:269914e0aa07 110
UCHITAKE 1:269914e0aa07 111 void Bot::controllMech()
UCHITAKE 1:269914e0aa07 112 {
number_key 25:d199d621ecca 113 //nishijoSword.move(1.0);
number_key 25:d199d621ecca 114 if(receiveSuccessed2) {
number_key 25:d199d621ecca 115 /*if(!pad1.getButton2(2)) tentacle.rightMove(TENTACLE_SPEED);
number_key 23:37bb9afe9fdc 116 if(!pad1.getButton2(3)) tentacle.rightMove(-TENTACLE_SPEED);
number_key 23:37bb9afe9fdc 117 if(pad1.getButton2(2) && pad1.getButton2(3)) tentacle.rightMove(0);
number_key 22:682cc376da6f 118
number_key 23:37bb9afe9fdc 119 if(!pad1.getButton2(0)) tentacle.leftMove(TENTACLE_SPEED);
number_key 23:37bb9afe9fdc 120 if(!pad1.getButton2(1)) tentacle.leftMove(-TENTACLE_SPEED);
number_key 25:d199d621ecca 121 if(pad1.getButton2(0) && pad1.getButton2(1)) tentacle.leftMove(0);*/
number_key 25:d199d621ecca 122
number_key 25:d199d621ecca 123 if(pad2.getStick(1) > 0){
number_key 25:d199d621ecca 124 tentacle.leftMove(pad2.getStick(1));
number_key 25:d199d621ecca 125 } else {
number_key 25:d199d621ecca 126 tentacle.leftMove(-TENTACLE_SPEED);
number_key 25:d199d621ecca 127 }
number_key 21:9c1061982b16 128
number_key 25:d199d621ecca 129 if(pad2.getStick(3) > 0){
number_key 25:d199d621ecca 130 tentacle.leftMove(pad2.getStick(3));
number_key 25:d199d621ecca 131 } else {
number_key 25:d199d621ecca 132 tentacle.leftMove(-TENTACLE_SPEED);
number_key 25:d199d621ecca 133 }
number_key 25:d199d621ecca 134
number_key 25:d199d621ecca 135 if (!pad1.getButton2(3)) nishijoSword.move(WIND_UP_SPEED);
number_key 25:d199d621ecca 136 if (!pad1.getButton2(5)) nishijoSword.move(-WIND_UP_SPEED);
number_key 25:d199d621ecca 137 if(pad2.getButton2(3) && pad2.getButton2(5)) nishijoSword.move(0);
number_key 25:d199d621ecca 138
number_key 25:d199d621ecca 139 //test = !pad1.getButton1(5);
number_key 25:d199d621ecca 140
number_key 25:d199d621ecca 141 //if(!pad1.getButton1(6)) powerSwitch = 0;
number_key 25:d199d621ecca 142 //if(!pad1.getButton1(5)&&!pad1.getButton1(4)) powerSwitch = 1;
number_key 25:d199d621ecca 143
number_key 24:9a4ca5442717 144 //else powerSwitch = 1;
number_key 22:682cc376da6f 145 } else {
number_key 23:37bb9afe9fdc 146 tentacle.stop();
number_key 25:d199d621ecca 147 nishijoSword.stop();
number_key 25:d199d621ecca 148 //test =0;
number_key 23:37bb9afe9fdc 149 //powerSwitch = 0;
number_key 22:682cc376da6f 150 }
number_key 23:37bb9afe9fdc 151 /*if(receiveSuccessed2){
number_key 23:37bb9afe9fdc 152
number_key 23:37bb9afe9fdc 153 }*/
number_key 23:37bb9afe9fdc 154 }
number_key 23:37bb9afe9fdc 155
number_key 23:37bb9afe9fdc 156
number_key 23:37bb9afe9fdc 157 void Bot::calibrate()
number_key 23:37bb9afe9fdc 158 {
uchitake 15:9a2dce34b660 159
uchitake 15:9a2dce34b660 160 }
number_key 22:682cc376da6f 161
number_key 23:37bb9afe9fdc 162 void Bot::debugPrint()
number_key 23:37bb9afe9fdc 163 {
number_key 23:37bb9afe9fdc 164 if(receiveSuccessed1) {
number_key 25:d199d621ecca 165 debugSerial.printf("%f %f %f %f \r\n",pad1.getStick(0),pad1.getStick(1),pad1.getStick(2),pad1.getStick(3));
number_key 25:d199d621ecca 166 } else {
number_key 25:d199d621ecca 167 debugSerial.printf("\r\n");
number_key 25:d199d621ecca 168 }
number_key 25:d199d621ecca 169 if(receiveSuccessed2) {
number_key 25:d199d621ecca 170 debugSerial.printf("%f %f %f %f \r\n",pad2.getStick(0),pad2.getStick(1),pad2.getStick(2),pad2.getStick(3));
number_key 25:d199d621ecca 171
number_key 23:37bb9afe9fdc 172 } else {
number_key 23:37bb9afe9fdc 173 debugSerial.printf("\r\n");
number_key 23:37bb9afe9fdc 174 }
number_key 22:682cc376da6f 175 }