NagaokaRoboticsClub_mbedTeam / Mbed OS NHK2017_octopus2_arms

タコ　腕

Dependencies:   2017NHKpin_config FEP ikarashiMDC

Fork of NHK2017_octopus2 by NagaokaRoboticsClub_mbedTeam

bot/bot.cpp@9:39be1525dfe0, 2017-09-07 (annotated)

Committer:
uchitake
Date:
Thu Sep 07 17:37:41 2017 +0900
Revision:
9:39be1525dfe0
Parent:
6:fe9767a50891
Child:
10:99dc4ae08998
Adapted newMDC

****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 1:845af5425eec 34 }
uchitake 1:845af5425eec 35
uchitake 1:845af5425eec 36 void Bot::confirmAll()
uchitake 1:845af5425eec 37 {
uchitake 6:fe9767a50891 38 receiveSuccessed = pad.receiveState();
uchitake 6:fe9767a50891 39 if(!receiveSuccessed) {
uchitake 9:39be1525dfe0 40 quadOmni.moveXY(0, 0, 0);
uchitake 9:39be1525dfe0 41 slider.slide(0);
uchitake 9:39be1525dfe0 42 for(int i = 0; i < 3; i++) {
uchitake 9:39be1525dfe0 43 armMotor[i].setSpeed(0);
uchitake 9:39be1525dfe0 44 }
uchitake 1:845af5425eec 45 }
uchitake 1:845af5425eec 46 }
uchitake 1:845af5425eec 47
uchitake 1:845af5425eec 48 void Bot::controllDrive()
uchitake 1:845af5425eec 49 {
uchitake 6:fe9767a50891 50 if(receiveSuccessed) {
uchitake 3:369d9ee17e84 51 if(pad.getNorm(1) > 0.5) {
uchitake 5:16ea97725085 52 PIDC::PID::setSetPoint((pad.getRadian(1) - M_PI / 2) * (180.0 / M_PI));
uchitake 3:369d9ee17e84 53 PIDC::confirm();
uchitake 3:369d9ee17e84 54 }
uchitake 9:39be1525dfe0 55 quadOmni.moveXY(
uchitake 6:fe9767a50891 56 pad.getStick(0) / 2.0,
uchitake 6:fe9767a50891 57 -pad.getStick(1) /2.0,
uchitake 6:fe9767a50891 58 PIDC::calculationResult
uchitake 6:fe9767a50891 59 );
uchitake 3:369d9ee17e84 60 } else {
uchitake 9:39be1525dfe0 61 quadOmni.moveXY(0, 0, 0);
uchitake 3:369d9ee17e84 62 }
uchitake 3:369d9ee17e84 63 }
uchitake 3:369d9ee17e84 64
uchitake 3:369d9ee17e84 65 void Bot::controllDrive2()
uchitake 3:369d9ee17e84 66 {
uchitake 4:1073deb368df 67 float moment = 0;
uchitake 3:369d9ee17e84 68 static float beforestick = pad.getStick(2);
uchitake 6:fe9767a50891 69
uchitake 5:16ea97725085 70 if(!pad.getButton2(2)) {
uchitake 6:fe9767a50891 71 PIDC::resetPlaneOffset();
uchitake 5:16ea97725085 72 }
uchitake 3:369d9ee17e84 73
uchitake 4:1073deb368df 74 if((beforestick >= 0.5 && pad.getStick(2) < 0.5) || (beforestick <= -0.5 && pad.getStick(2) > -0.5)) {
uchitake 5:16ea97725085 75 PIDC::PID::setSetPoint(0.0);
uchitake 6:fe9767a50891 76 PIDC::resetAxisOffset();
uchitake 3:369d9ee17e84 77 }
uchitake 3:369d9ee17e84 78
uchitake 4:1073deb368df 79 if(pad.getStick(2) > 0.5 || pad.getStick(2) < -0.5) {
uchitake 5:16ea97725085 80 moment = pad.getStick(2) / 4.0;
uchitake 3:369d9ee17e84 81 PIDC::confirm();
uchitake 4:1073deb368df 82 }
uchitake 4:1073deb368df 83 if(fabs(pad.getStick(2)) < 0.5) {
uchitake 3:369d9ee17e84 84 PIDC::confirm();
uchitake 6:fe9767a50891 85 moment = PIDC::calculationResult;
uchitake 3:369d9ee17e84 86 }
uchitake 3:369d9ee17e84 87
uchitake 6:fe9767a50891 88 if(receiveSuccessed) {
uchitake 9:39be1525dfe0 89 quadOmni.moveCircular(
uchitake 6:fe9767a50891 90 pad.getNorm(0) / 2.0,
uchitake 6:fe9767a50891 91 pad.getRadian(0) - PIDC::planeCurrentDegree / 10.0 * (M_PI / 180.0) + M_PI,
uchitake 6:fe9767a50891 92 moment
uchitake 6:fe9767a50891 93 );
uchitake 3:369d9ee17e84 94 } else {
uchitake 9:39be1525dfe0 95 quadOmni.moveXY(0, 0, 0);
uchitake 3:369d9ee17e84 96 }
uchitake 3:369d9ee17e84 97 beforestick = pad.getStick(2);
uchitake 1:845af5425eec 98 }
uchitake 1:845af5425eec 99
uchitake 1:845af5425eec 100 void Bot::controllMech()
uchitake 1:845af5425eec 101 {
uchitake 6:fe9767a50891 102 if(receiveSuccessed) {
uchitake 1:845af5425eec 103 // if(!pad.getButton1(0)) motor.moveSlider(ARM_MAX_SPEED);
uchitake 1:845af5425eec 104 // if(!pad.getButton1(1)) motor.moveSlider(-ARM_MAX_SPEED);
uchitake 1:845af5425eec 105 // if(pad.getButton1(0) && pad.getButton1(1)) motor.moveSlider(0);
uchitake 1:845af5425eec 106 //
uchitake 1:845af5425eec 107 // if(!pad.getButton1(3)) motor.shakeHead(ARM_MAX_SPEED);
uchitake 1:845af5425eec 108 // if(!pad.getButton1(4)) motor.shakeHead(-ARM_MAX_SPEED);
uchitake 1:845af5425eec 109 // if(pad.getButton1(3) && pad.getButton1(4)) motor.shakeHead(0);
uchitake 1:845af5425eec 110 //
uchitake 9:39be1525dfe0 111 if(!pad.getButton1(5)) armMotor[SWORD].setSpeed(0.6);
uchitake 9:39be1525dfe0 112 if(!pad.getButton1(6)) armMotor[SWORD].setSpeed(-0.6);
uchitake 9:39be1525dfe0 113 if(pad.getButton1(5) && pad.getButton1(6)) armMotor[SWORD].setSpeed(-0.0);
uchitake 1:845af5425eec 114
uchitake 1:845af5425eec 115 //
uchitake 1:845af5425eec 116 // if(!pad.getButton1(2)) {
uchitake 1:845af5425eec 117 // motor.destroy(DESTROY_MAX_SPEED);
uchitake 1:845af5425eec 118 // } else {
uchitake 1:845af5425eec 119 // motor.destroy(0);
uchitake 1:845af5425eec 120 // }
uchitake 1:845af5425eec 121 //
uchitake 9:39be1525dfe0 122 if(!pad.getButton2(1)) slider.release();
uchitake 9:39be1525dfe0 123 if(!pad.getButton2(3)) powerSwitch = 0;
uchitake 3:369d9ee17e84 124 } else {
uchitake 9:39be1525dfe0 125 slider.slide(0);
uchitake 9:39be1525dfe0 126 for(int i = 0; i < 3; i++) {
uchitake 9:39be1525dfe0 127 armMotor[i].setSpeed(0);
uchitake 9:39be1525dfe0 128 }
uchitake 3:369d9ee17e84 129 }
uchitake 1:845af5425eec 130 }
uchitake 1:845af5425eec 131
uchitake 1:845af5425eec 132
uchitake 1:845af5425eec 133 void Bot::calibrate()
uchitake 1:845af5425eec 134 {
uchitake 6:fe9767a50891 135 if(receiveSuccessed &&
uchitake 6:fe9767a50891 136 !pad.getButton2(0) &&
uchitake 6:fe9767a50891 137 !pad.getButton2(1)
uchitake 6:fe9767a50891 138 ) {
uchitake 1:845af5425eec 139 PIDC::calibration(HMC6352_ENTER_CALIB);
uchitake 9:39be1525dfe0 140 quadOmni.moveXY(0, 0, 0.4);
uchitake 3:369d9ee17e84 141 wait(5.0);
uchitake 9:39be1525dfe0 142 quadOmni.moveXY(0, 0, 0);
uchitake 1:845af5425eec 143 PIDC::calibration(HMC6352_EXIT_CALIB);
uchitake 1:845af5425eec 144 }
uchitake 3:369d9ee17e84 145 }