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@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?

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 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 }