![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
NHK2017 octopus robot
Dependencies: 2017NHKpin_config mbed FEP ikarashiMDC PID jy901 omni HMC6352 omni_wheel
Fork of KANI2017v2 by
bot/bot.cpp@24:9a4ca5442717, 2017-09-13 (annotated)
- Committer:
- number_key
- Date:
- Wed Sep 13 23:27:17 2017 +0900
- Revision:
- 24:9a4ca5442717
- Parent:
- 23:37bb9afe9fdc
- Child:
- 25:d199d621ecca
omni move
Who changed what in which revision?
User | Revision | Line number | New 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 |
23:37bb9afe9fdc | 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 |
23:37bb9afe9fdc | 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 |
23:37bb9afe9fdc | 19 | debugSerial(USBTX, USBRX, 115200) |
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 |
23:37bb9afe9fdc | 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 |
23:37bb9afe9fdc | 51 | if(receiveSuccessed1) { |
number_key | 22:682cc376da6f | 52 | if(pad1.getNorm(1) > 0.5) { |
number_key | 22:682cc376da6f | 53 | PIDC::PID::setSetPoint((pad1.getRadian(1) - M_PI / 2) * (180.0 / M_PI)); |
number_key | 22:682cc376da6f | 54 | PIDC::confirm(); |
number_key | 22:682cc376da6f | 55 | } |
number_key |
24:9a4ca5442717 | 56 | //PIDC::confirm(); |
number_key |
23:37bb9afe9fdc | 57 | PIDC::renewAngle(); |
number_key |
24:9a4ca5442717 | 58 | test = !test; |
number_key |
23:37bb9afe9fdc | 59 | quadOmni.moveXY( |
number_key |
24:9a4ca5442717 | 60 | pad1.getStick(0) * 0.8, |
number_key |
24:9a4ca5442717 | 61 | -pad1.getStick(1) * 0.8, |
number_key |
24:9a4ca5442717 | 62 | -pad1.getStick(2) * 0.4 |
number_key |
24:9a4ca5442717 | 63 | //PIDC::calculationResult |
number_key | 22:682cc376da6f | 64 | ); |
number_key |
24:9a4ca5442717 | 65 | /*quadOmni.moveCircular( |
number_key |
24:9a4ca5442717 | 66 | 0.2, |
number_key |
24:9a4ca5442717 | 67 | 0, |
number_key |
24:9a4ca5442717 | 68 | 0 |
number_key |
24:9a4ca5442717 | 69 | );*/ |
number_key | 22:682cc376da6f | 70 | } else { |
number_key |
23:37bb9afe9fdc | 71 | quadOmni.moveXY(0, 0, 0); |
number_key | 22:682cc376da6f | 72 | } |
number_key | 22:682cc376da6f | 73 | } |
number_key | 22:682cc376da6f | 74 | |
number_key | 22:682cc376da6f | 75 | void Bot::controllDrive2() |
number_key | 22:682cc376da6f | 76 | { |
number_key | 22:682cc376da6f | 77 | float moment = 0; |
number_key | 22:682cc376da6f | 78 | static float beforestick = pad1.getStick(2); |
number_key | 22:682cc376da6f | 79 | |
number_key | 22:682cc376da6f | 80 | if(!pad1.getButton2(2)) { |
number_key | 22:682cc376da6f | 81 | PIDC::resetPlaneOffset(); |
number_key | 22:682cc376da6f | 82 | } |
number_key | 22:682cc376da6f | 83 | |
number_key | 22:682cc376da6f | 84 | if((beforestick >= 0.5 && pad1.getStick(2) < 0.5) || (beforestick <= -0.5 && pad1.getStick(2) > -0.5)) { |
number_key | 22:682cc376da6f | 85 | PIDC::PID::setSetPoint(0.0); |
number_key | 22:682cc376da6f | 86 | PIDC::resetAxisOffset(); |
number_key | 22:682cc376da6f | 87 | } |
number_key | 22:682cc376da6f | 88 | |
number_key | 22:682cc376da6f | 89 | if(pad1.getStick(2) > 0.5 || pad1.getStick(2) < -0.5) { |
number_key | 22:682cc376da6f | 90 | moment = pad1.getStick(2) / 4.0; |
number_key | 22:682cc376da6f | 91 | PIDC::confirm(); |
number_key | 22:682cc376da6f | 92 | } |
number_key | 22:682cc376da6f | 93 | if(fabs(pad1.getStick(2)) < 0.5) { |
number_key | 22:682cc376da6f | 94 | PIDC::confirm(); |
number_key | 22:682cc376da6f | 95 | moment = PIDC::calculationResult; |
number_key | 22:682cc376da6f | 96 | } |
number_key | 22:682cc376da6f | 97 | |
number_key |
23:37bb9afe9fdc | 98 | if(receiveSuccessed1) { |
number_key |
23:37bb9afe9fdc | 99 | quadOmni.moveCircular( |
number_key | 22:682cc376da6f | 100 | pad1.getNorm(0) / 2.0, |
number_key | 22:682cc376da6f | 101 | pad1.getRadian(0) - PIDC::planeCurrentDegree / 10.0 * (M_PI / 180.0) + M_PI, |
number_key | 22:682cc376da6f | 102 | moment |
number_key | 22:682cc376da6f | 103 | ); |
number_key | 22:682cc376da6f | 104 | } else { |
number_key |
23:37bb9afe9fdc | 105 | quadOmni.moveXY(0, 0, 0); |
number_key | 22:682cc376da6f | 106 | } |
number_key | 22:682cc376da6f | 107 | beforestick = pad1.getStick(2); |
UCHITAKE | 1:269914e0aa07 | 108 | } |
UCHITAKE | 1:269914e0aa07 | 109 | |
UCHITAKE | 1:269914e0aa07 | 110 | void Bot::controllMech() |
UCHITAKE | 1:269914e0aa07 | 111 | { |
number_key |
23:37bb9afe9fdc | 112 | if(receiveSuccessed1) { |
number_key |
23:37bb9afe9fdc | 113 | if(!pad1.getButton2(2)) tentacle.rightMove(TENTACLE_SPEED); |
number_key |
23:37bb9afe9fdc | 114 | if(!pad1.getButton2(3)) tentacle.rightMove(-TENTACLE_SPEED); |
number_key |
23:37bb9afe9fdc | 115 | if(pad1.getButton2(2) && pad1.getButton2(3)) tentacle.rightMove(0); |
number_key | 22:682cc376da6f | 116 | |
number_key |
23:37bb9afe9fdc | 117 | if(!pad1.getButton2(0)) tentacle.leftMove(TENTACLE_SPEED); |
number_key |
23:37bb9afe9fdc | 118 | if(!pad1.getButton2(1)) tentacle.leftMove(-TENTACLE_SPEED); |
number_key |
23:37bb9afe9fdc | 119 | if(pad1.getButton2(0) && pad1.getButton2(1)) tentacle.leftMove(0); |
number_key |
21:9c1061982b16 | 120 | |
number_key |
24:9a4ca5442717 | 121 | if(!pad1.getButton1(2) && !pad1.getButton1(3) && !pad1.getButton1(4) && !pad1.getButton1(5)) powerSwitch = 0; |
number_key |
24:9a4ca5442717 | 122 | //else powerSwitch = 1; |
number_key | 22:682cc376da6f | 123 | } else { |
number_key |
23:37bb9afe9fdc | 124 | tentacle.stop(); |
number_key |
23:37bb9afe9fdc | 125 | //powerSwitch = 0; |
number_key | 22:682cc376da6f | 126 | } |
number_key |
23:37bb9afe9fdc | 127 | /*if(receiveSuccessed2){ |
number_key |
23:37bb9afe9fdc | 128 | |
number_key |
23:37bb9afe9fdc | 129 | }*/ |
number_key |
23:37bb9afe9fdc | 130 | } |
number_key |
23:37bb9afe9fdc | 131 | |
number_key |
23:37bb9afe9fdc | 132 | |
number_key |
23:37bb9afe9fdc | 133 | void Bot::calibrate() |
number_key |
23:37bb9afe9fdc | 134 | { |
uchitake |
15:9a2dce34b660 | 135 | |
uchitake |
15:9a2dce34b660 | 136 | } |
number_key | 22:682cc376da6f | 137 | |
number_key |
23:37bb9afe9fdc | 138 | void Bot::debugPrint() |
number_key |
23:37bb9afe9fdc | 139 | { |
number_key |
23:37bb9afe9fdc | 140 | if(receiveSuccessed1) { |
number_key |
24:9a4ca5442717 | 141 | //debugSerial.printf("%f %f %f %f \r\n",pad1.getStick(0),pad1.getStick(1),pad1.getStick(2),pad1.getStick(3)); |
number_key |
24:9a4ca5442717 | 142 | //debugSerial.printf("%f %f %f\r\n", PIDC::beforeDegree, PIDC::rawDegree, PIDC::calculationResult); |
number_key |
23:37bb9afe9fdc | 143 | } else { |
number_key |
23:37bb9afe9fdc | 144 | debugSerial.printf("\r\n"); |
number_key |
23:37bb9afe9fdc | 145 | } |
number_key | 22:682cc376da6f | 146 | } |