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@22:682cc376da6f, 2017-09-07 (annotated)

Committer:
number_key
Date:
Thu Sep 07 06:26:53 2017 +0000
Revision:
22:682cc376da6f
Parent:
21:9c1061982b16
Child:
23:37bb9afe9fdc
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 22:682cc376da6f 6 /* pad2(XBee2TX, XBee2RX, ADDR2),*/
number_key 22:682cc376da6f 7 motor(MDSDA, MDSCL)
UCHITAKE 0:757e9800c103 8 {
number_key 22:682cc376da6f 9 motor.goXY(0, 0, 0);
number_key 22:682cc376da6f 10 motor.moveRightTentacle(0);
number_key 22:682cc376da6f 11 motor.moveLeftTentacle(0);
number_key 22:682cc376da6f 12 motor.windUp(0);
UCHITAKE 1:269914e0aa07 13 }
UCHITAKE 1:269914e0aa07 14
UCHITAKE 1:269914e0aa07 15 void Bot::confirmAll()
UCHITAKE 1:269914e0aa07 16 {
number_key 22:682cc376da6f 17 receiveSuccessed = pad1.receiveState();
number_key 22:682cc376da6f 18 if(!receiveSuccessed) {
number_key 22:682cc376da6f 19 motor.goXY(0, 0, 0);
number_key 22:682cc376da6f 20 motor.moveLeftTentacle(0);
number_key 22:682cc376da6f 21 motor.moveRightTentacle(0);
number_key 22:682cc376da6f 22 motor.windUp(0);
number_key 22:682cc376da6f 23 }
number_key 22:682cc376da6f 24 //pad2.receiveState();
number_key 22:682cc376da6f 25 //PIDC::confirm();
number_key 22:682cc376da6f 26 //if(pad1.getNorm(1) > 0.5) PIDC::setSetPoint(pad1.getRadian(1) * (180.0 / M_PI));
UCHITAKE 1:269914e0aa07 27 }
UCHITAKE 1:269914e0aa07 28
UCHITAKE 1:269914e0aa07 29 void Bot::controllDrive()
UCHITAKE 1:269914e0aa07 30 {
number_key 22:682cc376da6f 31 //motor.goXY(pad1.getStick(0),pad1.getStick(1), PIDC::co);
number_key 22:682cc376da6f 32 if(receiveSuccessed) {
number_key 22:682cc376da6f 33 if(pad1.getNorm(1) > 0.5) {
number_key 22:682cc376da6f 34 PIDC::PID::setSetPoint((pad1.getRadian(1) - M_PI / 2) * (180.0 / M_PI));
number_key 22:682cc376da6f 35 PIDC::confirm();
number_key 22:682cc376da6f 36 }
number_key 22:682cc376da6f 37 motor.goXY(
number_key 22:682cc376da6f 38 pad1.getStick(0) / 2.0,
number_key 22:682cc376da6f 39 -pad1.getStick(1) /2.0,
number_key 22:682cc376da6f 40 PIDC::calculationResult
number_key 22:682cc376da6f 41 );
number_key 22:682cc376da6f 42 } else {
number_key 22:682cc376da6f 43 motor.goXY(0, 0, 0);
number_key 22:682cc376da6f 44 }
number_key 22:682cc376da6f 45 }
number_key 22:682cc376da6f 46
number_key 22:682cc376da6f 47 void Bot::controllDrive2()
number_key 22:682cc376da6f 48 {
number_key 22:682cc376da6f 49 float moment = 0;
number_key 22:682cc376da6f 50 static float beforestick = pad1.getStick(2);
number_key 22:682cc376da6f 51
number_key 22:682cc376da6f 52 if(!pad1.getButton2(2)) {
number_key 22:682cc376da6f 53 PIDC::resetPlaneOffset();
number_key 22:682cc376da6f 54 }
number_key 22:682cc376da6f 55
number_key 22:682cc376da6f 56 if((beforestick >= 0.5 && pad1.getStick(2) < 0.5) || (beforestick <= -0.5 && pad1.getStick(2) > -0.5)) {
number_key 22:682cc376da6f 57 PIDC::PID::setSetPoint(0.0);
number_key 22:682cc376da6f 58 PIDC::resetAxisOffset();
number_key 22:682cc376da6f 59 }
number_key 22:682cc376da6f 60
number_key 22:682cc376da6f 61 if(pad1.getStick(2) > 0.5 || pad1.getStick(2) < -0.5) {
number_key 22:682cc376da6f 62 moment = pad1.getStick(2) / 4.0;
number_key 22:682cc376da6f 63 PIDC::confirm();
number_key 22:682cc376da6f 64 }
number_key 22:682cc376da6f 65 if(fabs(pad1.getStick(2)) < 0.5) {
number_key 22:682cc376da6f 66 PIDC::confirm();
number_key 22:682cc376da6f 67 moment = PIDC::calculationResult;
number_key 22:682cc376da6f 68 }
number_key 22:682cc376da6f 69
number_key 22:682cc376da6f 70 if(receiveSuccessed) {
number_key 22:682cc376da6f 71 motor.goCircular(
number_key 22:682cc376da6f 72 pad1.getNorm(0) / 2.0,
number_key 22:682cc376da6f 73 pad1.getRadian(0) - PIDC::planeCurrentDegree / 10.0 * (M_PI / 180.0) + M_PI,
number_key 22:682cc376da6f 74 moment
number_key 22:682cc376da6f 75 );
number_key 22:682cc376da6f 76 } else {
number_key 22:682cc376da6f 77 motor.goXY(0, 0, 0);
number_key 22:682cc376da6f 78 }
number_key 22:682cc376da6f 79 beforestick = pad1.getStick(2);
UCHITAKE 1:269914e0aa07 80 }
UCHITAKE 1:269914e0aa07 81
UCHITAKE 1:269914e0aa07 82 void Bot::controllMech()
UCHITAKE 1:269914e0aa07 83 {
number_key 22:682cc376da6f 84 if(receiveSuccessed){
number_key 22:682cc376da6f 85 if(!pad1.getButton1(0)) motor.moveRightTentacle(TENTACLE_MAX_SPEED);
number_key 22:682cc376da6f 86 if(!pad1.getButton1(1)) motor.moveRightTentacle(-TENTACLE_MAX_SPEED);
number_key 22:682cc376da6f 87 if(pad1.getButton1(0) && pad1.getButton1(1)) motor.moveRightTentacle(0);
number_key 22:682cc376da6f 88
number_key 22:682cc376da6f 89 if(!pad1.getButton1(2)) motor.moveLeftTentacle(TENTACLE_MAX_SPEED);
number_key 22:682cc376da6f 90 if(!pad1.getButton1(3)) motor.moveLeftTentacle(-TENTACLE_MAX_SPEED);
number_key 22:682cc376da6f 91 if(pad1.getButton1(2) && pad1.getButton1(3)) motor.moveLeftTentacle(0);
number_key 21:9c1061982b16 92
number_key 22:682cc376da6f 93 if(!pad1.getButton1(4)) motor.windUp(WIND_UP_SPEED);
number_key 22:682cc376da6f 94 if(!pad1.getButton1(5)) motor.windUp(-WIND_UP_SPEED);
number_key 22:682cc376da6f 95 if(pad1.getButton1(4) && pad1.getButton1(5)) motor.windUp(0);
UCHITAKE 1:269914e0aa07 96
number_key 22:682cc376da6f 97 } else {
number_key 22:682cc376da6f 98 motor.moveLeftTentacle(0);
number_key 22:682cc376da6f 99 motor.moveRightTentacle(0);
number_key 22:682cc376da6f 100 motor.windUp(0);
number_key 22:682cc376da6f 101 }
uchitake 15:9a2dce34b660 102
uchitake 15:9a2dce34b660 103 }
number_key 22:682cc376da6f 104
number_key 22:682cc376da6f 105 void Bot::calibrate(){
number_key 22:682cc376da6f 106
number_key 22:682cc376da6f 107 }