NagaokaRoboticsClub_mbedTeam / Mbed OS NHK2017_octopus2_arms

タコ　腕

Dependencies:   2017NHKpin_config FEP ikarashiMDC

Fork of NHK2017_octopus2 by NagaokaRoboticsClub_mbedTeam

bot/bot.cpp@10:99dc4ae08998, 2017-09-07 (annotated)

Committer:
UCHITAKE
Date:
Thu Sep 07 18:20:15 2017 +0900
Revision:
10:99dc4ae08998
Parent:
9:39be1525dfe0
Child:
11:a8385ca0a275
fix

+++NO_WORKING+++

Who changed what in which revision?

UserRevisionLine numberNew contents of line
uchitake 1:845af5425eec 1 #include "bot.h"
uchitake 1:845af5425eec 2
uchitake 1:845af5425eec 3 Bot::Bot() :
uchitake 6:fe9767a50891 4 PIDC(),
uchitake 6:fe9767a50891 5 pad(XBee1TX, XBee1RX, ADDR),
uchitake 9:39be1525dfe0 6 RS485(MDTX, MDRX, 38400),
uchitake 9:39be1525dfe0 7 RS485Controller(PWM1),
uchitake 9:39be1525dfe0 8 powerSwitch(PWM2),
uchitake 9:39be1525dfe0 9 quadOmni(&RS485Controller, &RS485),
uchitake 9:39be1525dfe0 10 slider(&RS485Controller, &RS485),
uchitake 9:39be1525dfe0 11 armMotor({
uchitake 9:39be1525dfe0 12 {&RS485Controller, 1, 1, SM, &RS485},
uchitake 9:39be1525dfe0 13 {&RS485Controller, 1, 2, SM, &RS485},
uchitake 9:39be1525dfe0 14 {&RS485Controller, 1, 3, SM, &RS485}
uchitake 9:39be1525dfe0 15 }),
uchitake 9:39be1525dfe0 16 receiveSuccessed(0),
uchitake 9:39be1525dfe0 17 led({DebugLED1, DebugLED2, DebugLED3, DebugLED4,
uchitake 9:39be1525dfe0 18 DebugLED5, DebugLED6, DebugLED7, DebugLED8}),
uchitake 9:39be1525dfe0 19 debugSerial(USBTX, USBRX, 115200)
uchitake 1:845af5425eec 20 {
uchitake 9:39be1525dfe0 21 for(int i = 0; i < 3; i++) {
uchitake 9:39be1525dfe0 22 armMotor[i].braking = true;
uchitake 9:39be1525dfe0 23 }
uchitake 9:39be1525dfe0 24
uchitake 9:39be1525dfe0 25 quadOmni.moveXY(0, 0, 0);
uchitake 9:39be1525dfe0 26
uchitake 9:39be1525dfe0 27 powerSwitch = 1;
uchitake 9:39be1525dfe0 28
uchitake 9:39be1525dfe0 29 for(int i = 0; i < 8; i++) {
uchitake 9:39be1525dfe0 30 led[i] = 1;
uchitake 9:39be1525dfe0 31 wait(0.1);
uchitake 9:39be1525dfe0 32 led[i] = 0;
uchitake 9:39be1525dfe0 33 }
UCHITAKE 10:99dc4ae08998 34 for(int i = 0; i < 8; i++) {
UCHITAKE 10:99dc4ae08998 35 led[i] = 1;
UCHITAKE 10:99dc4ae08998 36 wait(0.1);
UCHITAKE 10:99dc4ae08998 37 led[i] = 0;
UCHITAKE 10:99dc4ae08998 38 }
uchitake 1:845af5425eec 39 }
uchitake 1:845af5425eec 40
uchitake 1:845af5425eec 41 void Bot::confirmAll()
uchitake 1:845af5425eec 42 {
uchitake 6:fe9767a50891 43 receiveSuccessed = pad.receiveState();
uchitake 6:fe9767a50891 44 if(!receiveSuccessed) {
uchitake 9:39be1525dfe0 45 quadOmni.moveXY(0, 0, 0);
uchitake 9:39be1525dfe0 46 slider.slide(0);
uchitake 9:39be1525dfe0 47 for(int i = 0; i < 3; i++) {
uchitake 9:39be1525dfe0 48 armMotor[i].setSpeed(0);
uchitake 9:39be1525dfe0 49 }
uchitake 1:845af5425eec 50 }
uchitake 1:845af5425eec 51 }
uchitake 1:845af5425eec 52
uchitake 1:845af5425eec 53 void Bot::controllDrive()
uchitake 1:845af5425eec 54 {
uchitake 6:fe9767a50891 55 if(receiveSuccessed) {
uchitake 3:369d9ee17e84 56 if(pad.getNorm(1) > 0.5) {
uchitake 5:16ea97725085 57 PIDC::PID::setSetPoint((pad.getRadian(1) - M_PI / 2) * (180.0 / M_PI));
uchitake 3:369d9ee17e84 58 PIDC::confirm();
uchitake 3:369d9ee17e84 59 }
uchitake 9:39be1525dfe0 60 quadOmni.moveXY(
uchitake 6:fe9767a50891 61 pad.getStick(0) / 2.0,
uchitake 6:fe9767a50891 62 -pad.getStick(1) /2.0,
uchitake 6:fe9767a50891 63 PIDC::calculationResult
uchitake 6:fe9767a50891 64 );
uchitake 3:369d9ee17e84 65 } else {
uchitake 9:39be1525dfe0 66 quadOmni.moveXY(0, 0, 0);
uchitake 3:369d9ee17e84 67 }
uchitake 3:369d9ee17e84 68 }
uchitake 3:369d9ee17e84 69
uchitake 3:369d9ee17e84 70 void Bot::controllDrive2()
uchitake 3:369d9ee17e84 71 {
uchitake 4:1073deb368df 72 float moment = 0;
uchitake 3:369d9ee17e84 73 static float beforestick = pad.getStick(2);
uchitake 6:fe9767a50891 74
uchitake 5:16ea97725085 75 if(!pad.getButton2(2)) {
uchitake 6:fe9767a50891 76 PIDC::resetPlaneOffset();
uchitake 5:16ea97725085 77 }
uchitake 3:369d9ee17e84 78
uchitake 4:1073deb368df 79 if((beforestick >= 0.5 && pad.getStick(2) < 0.5) || (beforestick <= -0.5 && pad.getStick(2) > -0.5)) {
uchitake 5:16ea97725085 80 PIDC::PID::setSetPoint(0.0);
uchitake 6:fe9767a50891 81 PIDC::resetAxisOffset();
uchitake 3:369d9ee17e84 82 }
uchitake 3:369d9ee17e84 83
uchitake 4:1073deb368df 84 if(pad.getStick(2) > 0.5 || pad.getStick(2) < -0.5) {
uchitake 5:16ea97725085 85 moment = pad.getStick(2) / 4.0;
uchitake 3:369d9ee17e84 86 PIDC::confirm();
uchitake 4:1073deb368df 87 }
uchitake 4:1073deb368df 88 if(fabs(pad.getStick(2)) < 0.5) {
uchitake 3:369d9ee17e84 89 PIDC::confirm();
uchitake 6:fe9767a50891 90 moment = PIDC::calculationResult;
uchitake 3:369d9ee17e84 91 }
uchitake 3:369d9ee17e84 92
uchitake 6:fe9767a50891 93 if(receiveSuccessed) {
uchitake 9:39be1525dfe0 94 quadOmni.moveCircular(
uchitake 6:fe9767a50891 95 pad.getNorm(0) / 2.0,
uchitake 6:fe9767a50891 96 pad.getRadian(0) - PIDC::planeCurrentDegree / 10.0 * (M_PI / 180.0) + M_PI,
uchitake 6:fe9767a50891 97 moment
uchitake 6:fe9767a50891 98 );
uchitake 3:369d9ee17e84 99 } else {
uchitake 9:39be1525dfe0 100 quadOmni.moveXY(0, 0, 0);
uchitake 3:369d9ee17e84 101 }
uchitake 3:369d9ee17e84 102 beforestick = pad.getStick(2);
uchitake 1:845af5425eec 103 }
uchitake 1:845af5425eec 104
uchitake 1:845af5425eec 105 void Bot::controllMech()
uchitake 1:845af5425eec 106 {
uchitake 6:fe9767a50891 107 if(receiveSuccessed) {
uchitake 1:845af5425eec 108 // if(!pad.getButton1(0)) motor.moveSlider(ARM_MAX_SPEED);
uchitake 1:845af5425eec 109 // if(!pad.getButton1(1)) motor.moveSlider(-ARM_MAX_SPEED);
uchitake 1:845af5425eec 110 // if(pad.getButton1(0) && pad.getButton1(1)) motor.moveSlider(0);
uchitake 1:845af5425eec 111 //
uchitake 1:845af5425eec 112 // if(!pad.getButton1(3)) motor.shakeHead(ARM_MAX_SPEED);
uchitake 1:845af5425eec 113 // if(!pad.getButton1(4)) motor.shakeHead(-ARM_MAX_SPEED);
uchitake 1:845af5425eec 114 // if(pad.getButton1(3) && pad.getButton1(4)) motor.shakeHead(0);
uchitake 1:845af5425eec 115 //
uchitake 9:39be1525dfe0 116 if(!pad.getButton1(5)) armMotor[SWORD].setSpeed(0.6);
uchitake 9:39be1525dfe0 117 if(!pad.getButton1(6)) armMotor[SWORD].setSpeed(-0.6);
uchitake 9:39be1525dfe0 118 if(pad.getButton1(5) && pad.getButton1(6)) armMotor[SWORD].setSpeed(-0.0);
uchitake 1:845af5425eec 119
uchitake 1:845af5425eec 120 //
uchitake 1:845af5425eec 121 // if(!pad.getButton1(2)) {
uchitake 1:845af5425eec 122 // motor.destroy(DESTROY_MAX_SPEED);
uchitake 1:845af5425eec 123 // } else {
uchitake 1:845af5425eec 124 // motor.destroy(0);
uchitake 1:845af5425eec 125 // }
uchitake 1:845af5425eec 126 //
uchitake 9:39be1525dfe0 127 if(!pad.getButton2(1)) slider.release();
uchitake 9:39be1525dfe0 128 if(!pad.getButton2(3)) powerSwitch = 0;
uchitake 3:369d9ee17e84 129 } else {
uchitake 9:39be1525dfe0 130 slider.slide(0);
uchitake 9:39be1525dfe0 131 for(int i = 0; i < 3; i++) {
uchitake 9:39be1525dfe0 132 armMotor[i].setSpeed(0);
uchitake 9:39be1525dfe0 133 }
uchitake 3:369d9ee17e84 134 }
uchitake 1:845af5425eec 135 }
uchitake 1:845af5425eec 136
uchitake 1:845af5425eec 137
uchitake 1:845af5425eec 138 void Bot::calibrate()
uchitake 1:845af5425eec 139 {
uchitake 6:fe9767a50891 140 if(receiveSuccessed &&
uchitake 6:fe9767a50891 141 !pad.getButton2(0) &&
uchitake 6:fe9767a50891 142 !pad.getButton2(1)
uchitake 6:fe9767a50891 143 ) {
uchitake 1:845af5425eec 144 PIDC::calibration(HMC6352_ENTER_CALIB);
uchitake 9:39be1525dfe0 145 quadOmni.moveXY(0, 0, 0.4);
uchitake 3:369d9ee17e84 146 wait(5.0);
uchitake 9:39be1525dfe0 147 quadOmni.moveXY(0, 0, 0);
uchitake 1:845af5425eec 148 PIDC::calibration(HMC6352_EXIT_CALIB);
uchitake 1:845af5425eec 149 }
uchitake 3:369d9ee17e84 150 }