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

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