NHK2017 octopus robot
Dependencies: 2017NHKpin_config mbed FEP ikarashiMDC PID jy901 omni HMC6352 omni_wheel
Fork of KANI2017v2 by
bot/bot.cpp@23:37bb9afe9fdc, 2017-09-13 (annotated)
- Committer:
- number_key
- Date:
- Wed Sep 13 14:26:47 2017 +0900
- Revision:
- 23:37bb9afe9fdc
- Parent:
- 22:682cc376da6f
- Child:
- 24:9a4ca5442717
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 |
23:37bb9afe9fdc | 8 | RS485Controller(PWM1), |
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 |
23:37bb9afe9fdc | 18 | debugSerial(USBTX, USBRX, 115200) |
UCHITAKE | 0:757e9800c103 | 19 | { |
number_key |
23:37bb9afe9fdc | 20 | |
number_key |
23:37bb9afe9fdc | 21 | |
number_key |
23:37bb9afe9fdc | 22 | quadOmni.moveXY(0, 0, 0); |
number_key |
23:37bb9afe9fdc | 23 | |
number_key |
23:37bb9afe9fdc | 24 | powerSwitch = 1; |
number_key |
23:37bb9afe9fdc | 25 | |
number_key |
23:37bb9afe9fdc | 26 | for(int i = 0; i < 8; i++) { |
number_key |
23:37bb9afe9fdc | 27 | led[i] = 1; |
number_key |
23:37bb9afe9fdc | 28 | wait(0.1); |
number_key |
23:37bb9afe9fdc | 29 | led[i] = 0; |
number_key |
23:37bb9afe9fdc | 30 | } |
number_key |
23:37bb9afe9fdc | 31 | for(int i = 0; i < 8; i++) { |
number_key |
23:37bb9afe9fdc | 32 | led[i] = 1; |
number_key |
23:37bb9afe9fdc | 33 | wait(0.1); |
number_key |
23:37bb9afe9fdc | 34 | led[i] = 0; |
number_key |
23:37bb9afe9fdc | 35 | } |
UCHITAKE | 1:269914e0aa07 | 36 | } |
UCHITAKE | 1:269914e0aa07 | 37 | |
UCHITAKE | 1:269914e0aa07 | 38 | void Bot::confirmAll() |
UCHITAKE | 1:269914e0aa07 | 39 | { |
number_key |
23:37bb9afe9fdc | 40 | receiveSuccessed1 = pad1.receiveState(); |
number_key |
23:37bb9afe9fdc | 41 | //receiveSuccessed2 = pad2.receiveState(); |
number_key |
23:37bb9afe9fdc | 42 | if(!receiveSuccessed1) { |
number_key |
23:37bb9afe9fdc | 43 | quadOmni.moveXY(0, 0, 0); |
number_key |
23:37bb9afe9fdc | 44 | tentacle.stop(); |
number_key | 22:682cc376da6f | 45 | } |
UCHITAKE | 1:269914e0aa07 | 46 | } |
UCHITAKE | 1:269914e0aa07 | 47 | |
UCHITAKE | 1:269914e0aa07 | 48 | void Bot::controllDrive() |
UCHITAKE | 1:269914e0aa07 | 49 | { |
number_key |
23:37bb9afe9fdc | 50 | if(receiveSuccessed1) { |
number_key | 22:682cc376da6f | 51 | if(pad1.getNorm(1) > 0.5) { |
number_key | 22:682cc376da6f | 52 | PIDC::PID::setSetPoint((pad1.getRadian(1) - M_PI / 2) * (180.0 / M_PI)); |
number_key | 22:682cc376da6f | 53 | PIDC::confirm(); |
number_key | 22:682cc376da6f | 54 | } |
number_key |
23:37bb9afe9fdc | 55 | PIDC::renewAngle(); |
number_key |
23:37bb9afe9fdc | 56 | quadOmni.moveXY( |
number_key | 22:682cc376da6f | 57 | pad1.getStick(0) / 2.0, |
number_key | 22:682cc376da6f | 58 | -pad1.getStick(1) /2.0, |
number_key | 22:682cc376da6f | 59 | PIDC::calculationResult |
number_key | 22:682cc376da6f | 60 | ); |
number_key | 22:682cc376da6f | 61 | } else { |
number_key |
23:37bb9afe9fdc | 62 | quadOmni.moveXY(0, 0, 0); |
number_key | 22:682cc376da6f | 63 | } |
number_key | 22:682cc376da6f | 64 | } |
number_key | 22:682cc376da6f | 65 | |
number_key | 22:682cc376da6f | 66 | void Bot::controllDrive2() |
number_key | 22:682cc376da6f | 67 | { |
number_key | 22:682cc376da6f | 68 | float moment = 0; |
number_key | 22:682cc376da6f | 69 | static float beforestick = pad1.getStick(2); |
number_key | 22:682cc376da6f | 70 | |
number_key | 22:682cc376da6f | 71 | if(!pad1.getButton2(2)) { |
number_key | 22:682cc376da6f | 72 | PIDC::resetPlaneOffset(); |
number_key | 22:682cc376da6f | 73 | } |
number_key | 22:682cc376da6f | 74 | |
number_key | 22:682cc376da6f | 75 | if((beforestick >= 0.5 && pad1.getStick(2) < 0.5) || (beforestick <= -0.5 && pad1.getStick(2) > -0.5)) { |
number_key | 22:682cc376da6f | 76 | PIDC::PID::setSetPoint(0.0); |
number_key | 22:682cc376da6f | 77 | PIDC::resetAxisOffset(); |
number_key | 22:682cc376da6f | 78 | } |
number_key | 22:682cc376da6f | 79 | |
number_key | 22:682cc376da6f | 80 | if(pad1.getStick(2) > 0.5 || pad1.getStick(2) < -0.5) { |
number_key | 22:682cc376da6f | 81 | moment = pad1.getStick(2) / 4.0; |
number_key | 22:682cc376da6f | 82 | PIDC::confirm(); |
number_key | 22:682cc376da6f | 83 | } |
number_key | 22:682cc376da6f | 84 | if(fabs(pad1.getStick(2)) < 0.5) { |
number_key | 22:682cc376da6f | 85 | PIDC::confirm(); |
number_key | 22:682cc376da6f | 86 | moment = PIDC::calculationResult; |
number_key | 22:682cc376da6f | 87 | } |
number_key | 22:682cc376da6f | 88 | |
number_key |
23:37bb9afe9fdc | 89 | if(receiveSuccessed1) { |
number_key |
23:37bb9afe9fdc | 90 | quadOmni.moveCircular( |
number_key | 22:682cc376da6f | 91 | pad1.getNorm(0) / 2.0, |
number_key | 22:682cc376da6f | 92 | pad1.getRadian(0) - PIDC::planeCurrentDegree / 10.0 * (M_PI / 180.0) + M_PI, |
number_key | 22:682cc376da6f | 93 | moment |
number_key | 22:682cc376da6f | 94 | ); |
number_key | 22:682cc376da6f | 95 | } else { |
number_key |
23:37bb9afe9fdc | 96 | quadOmni.moveXY(0, 0, 0); |
number_key | 22:682cc376da6f | 97 | } |
number_key | 22:682cc376da6f | 98 | beforestick = pad1.getStick(2); |
UCHITAKE | 1:269914e0aa07 | 99 | } |
UCHITAKE | 1:269914e0aa07 | 100 | |
UCHITAKE | 1:269914e0aa07 | 101 | void Bot::controllMech() |
UCHITAKE | 1:269914e0aa07 | 102 | { |
number_key |
23:37bb9afe9fdc | 103 | if(receiveSuccessed1) { |
number_key |
23:37bb9afe9fdc | 104 | if(!pad1.getButton2(2)) tentacle.rightMove(TENTACLE_SPEED); |
number_key |
23:37bb9afe9fdc | 105 | if(!pad1.getButton2(3)) tentacle.rightMove(-TENTACLE_SPEED); |
number_key |
23:37bb9afe9fdc | 106 | if(pad1.getButton2(2) && pad1.getButton2(3)) tentacle.rightMove(0); |
number_key | 22:682cc376da6f | 107 | |
number_key |
23:37bb9afe9fdc | 108 | if(!pad1.getButton2(0)) tentacle.leftMove(TENTACLE_SPEED); |
number_key |
23:37bb9afe9fdc | 109 | if(!pad1.getButton2(1)) tentacle.leftMove(-TENTACLE_SPEED); |
number_key |
23:37bb9afe9fdc | 110 | if(pad1.getButton2(0) && pad1.getButton2(1)) tentacle.leftMove(0); |
number_key |
21:9c1061982b16 | 111 | |
number_key |
23:37bb9afe9fdc | 112 | //if(!pad1.getButton1(0)); powerSwitch = 0; |
number_key | 22:682cc376da6f | 113 | } else { |
number_key |
23:37bb9afe9fdc | 114 | tentacle.stop(); |
number_key |
23:37bb9afe9fdc | 115 | //powerSwitch = 0; |
number_key | 22:682cc376da6f | 116 | } |
number_key |
23:37bb9afe9fdc | 117 | /*if(receiveSuccessed2){ |
number_key |
23:37bb9afe9fdc | 118 | |
number_key |
23:37bb9afe9fdc | 119 | }*/ |
number_key |
23:37bb9afe9fdc | 120 | } |
number_key |
23:37bb9afe9fdc | 121 | |
number_key |
23:37bb9afe9fdc | 122 | |
number_key |
23:37bb9afe9fdc | 123 | void Bot::calibrate() |
number_key |
23:37bb9afe9fdc | 124 | { |
uchitake |
15:9a2dce34b660 | 125 | |
uchitake |
15:9a2dce34b660 | 126 | } |
number_key | 22:682cc376da6f | 127 | |
number_key |
23:37bb9afe9fdc | 128 | void Bot::debugPrint() |
number_key |
23:37bb9afe9fdc | 129 | { |
number_key |
23:37bb9afe9fdc | 130 | if(receiveSuccessed1) { |
number_key |
23:37bb9afe9fdc | 131 | //debugSerial.printf("%f %f %f %f ,%f\r\n",pad1.getStick(0),pad1.getStick(1),pad1.getStick(2),pad1.getStick(3)); |
number_key |
23:37bb9afe9fdc | 132 | debugSerial.printf("%f\r\n",PIDC::angle); |
number_key |
23:37bb9afe9fdc | 133 | } else { |
number_key |
23:37bb9afe9fdc | 134 | debugSerial.printf("\r\n"); |
number_key |
23:37bb9afe9fdc | 135 | } |
number_key | 22:682cc376da6f | 136 | } |