NagaokaRoboticsClub_mbedTeam / Mbed OS NHK2017_octopus2_arms

タコ　腕

Dependencies:   2017NHKpin_config FEP ikarashiMDC

Fork of NHK2017_octopus2 by NagaokaRoboticsClub_mbedTeam

bot/bot.cpp@14:1fadf7d2f583, 2017-09-12 (annotated)

Committer:
uchitake
Date:
Tue Sep 12 10:21:00 2017 +0900
Revision:
14:1fadf7d2f583
Parent:
11:a8385ca0a275
Child:
15:9aa11febe517
fix leds

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 11:a8385ca0a275 12 ikarashiMDC({&RS485Controller, 1, 1, SM, &RS485}),
UCHITAKE 11:a8385ca0a275 13 ikarashiMDC({&RS485Controller, 1, 2, SM, &RS485}),
UCHITAKE 11:a8385ca0a275 14 ikarashiMDC({&RS485Controller, 1, 3, SM, &RS485})
uchitake 9:39be1525dfe0 15 }),
uchitake 9:39be1525dfe0 16 receiveSuccessed(0),
uchitake 14:1fadf7d2f583 17 led({DebugLED3, DebugLED4, DebugLED5}),
uchitake 9:39be1525dfe0 18 debugSerial(USBTX, USBRX, 115200)
uchitake 1:845af5425eec 19 {
uchitake 9:39be1525dfe0 20 for(int i = 0; i < 3; i++) {
uchitake 9:39be1525dfe0 21 armMotor[i].braking = true;
uchitake 9:39be1525dfe0 22 }
uchitake 9:39be1525dfe0 23
uchitake 9:39be1525dfe0 24 quadOmni.moveXY(0, 0, 0);
uchitake 9:39be1525dfe0 25
uchitake 9:39be1525dfe0 26 powerSwitch = 1;
uchitake 14:1fadf7d2f583 27 for(int i = 0; i < 3; i++) {
uchitake 14:1fadf7d2f583 28 led[i] = true;
uchitake 9:39be1525dfe0 29 wait(0.1);
uchitake 14:1fadf7d2f583 30 led[i] = false;
UCHITAKE 10:99dc4ae08998 31 }
uchitake 1:845af5425eec 32 }
uchitake 1:845af5425eec 33
uchitake 1:845af5425eec 34 void Bot::confirmAll()
uchitake 1:845af5425eec 35 {
uchitake 6:fe9767a50891 36 receiveSuccessed = pad.receiveState();
uchitake 14:1fadf7d2f583 37 led[0] = receiveSuccessed;
uchitake 6:fe9767a50891 38 if(!receiveSuccessed) {
uchitake 9:39be1525dfe0 39 quadOmni.moveXY(0, 0, 0);
uchitake 9:39be1525dfe0 40 slider.slide(0);
uchitake 9:39be1525dfe0 41 for(int i = 0; i < 3; i++) {
uchitake 9:39be1525dfe0 42 armMotor[i].setSpeed(0);
uchitake 9:39be1525dfe0 43 }
uchitake 1:845af5425eec 44 }
uchitake 1:845af5425eec 45 }
uchitake 1:845af5425eec 46
uchitake 1:845af5425eec 47 void Bot::controllDrive()
uchitake 1:845af5425eec 48 {
uchitake 6:fe9767a50891 49 if(receiveSuccessed) {
uchitake 14:1fadf7d2f583 50 led[1] = !led[1];
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 14:1fadf7d2f583 89 led[1] = !led[1];
uchitake 9:39be1525dfe0 90 quadOmni.moveCircular(
uchitake 6:fe9767a50891 91 pad.getNorm(0) / 2.0,
uchitake 6:fe9767a50891 92 pad.getRadian(0) - PIDC::planeCurrentDegree / 10.0 * (M_PI / 180.0) + M_PI,
uchitake 6:fe9767a50891 93 moment
uchitake 6:fe9767a50891 94 );
uchitake 3:369d9ee17e84 95 } else {
uchitake 9:39be1525dfe0 96 quadOmni.moveXY(0, 0, 0);
uchitake 3:369d9ee17e84 97 }
uchitake 3:369d9ee17e84 98 beforestick = pad.getStick(2);
uchitake 1:845af5425eec 99 }
uchitake 1:845af5425eec 100
uchitake 1:845af5425eec 101 void Bot::controllMech()
uchitake 1:845af5425eec 102 {
uchitake 6:fe9767a50891 103 if(receiveSuccessed) {
uchitake 1:845af5425eec 104 // if(!pad.getButton1(0)) motor.moveSlider(ARM_MAX_SPEED);
uchitake 1:845af5425eec 105 // if(!pad.getButton1(1)) motor.moveSlider(-ARM_MAX_SPEED);
uchitake 1:845af5425eec 106 // if(pad.getButton1(0) && pad.getButton1(1)) motor.moveSlider(0);
uchitake 1:845af5425eec 107 //
uchitake 1:845af5425eec 108 // if(!pad.getButton1(3)) motor.shakeHead(ARM_MAX_SPEED);
uchitake 1:845af5425eec 109 // if(!pad.getButton1(4)) motor.shakeHead(-ARM_MAX_SPEED);
uchitake 1:845af5425eec 110 // if(pad.getButton1(3) && pad.getButton1(4)) motor.shakeHead(0);
uchitake 1:845af5425eec 111 //
uchitake 9:39be1525dfe0 112 if(!pad.getButton1(5)) armMotor[SWORD].setSpeed(0.6);
uchitake 9:39be1525dfe0 113 if(!pad.getButton1(6)) armMotor[SWORD].setSpeed(-0.6);
uchitake 9:39be1525dfe0 114 if(pad.getButton1(5) && pad.getButton1(6)) armMotor[SWORD].setSpeed(-0.0);
uchitake 1:845af5425eec 115
uchitake 1:845af5425eec 116 //
uchitake 1:845af5425eec 117 // if(!pad.getButton1(2)) {
uchitake 1:845af5425eec 118 // motor.destroy(DESTROY_MAX_SPEED);
uchitake 1:845af5425eec 119 // } else {
uchitake 1:845af5425eec 120 // motor.destroy(0);
uchitake 1:845af5425eec 121 // }
uchitake 1:845af5425eec 122 //
uchitake 9:39be1525dfe0 123 if(!pad.getButton2(1)) slider.release();
uchitake 9:39be1525dfe0 124 if(!pad.getButton2(3)) powerSwitch = 0;
uchitake 3:369d9ee17e84 125 } else {
uchitake 9:39be1525dfe0 126 slider.slide(0);
uchitake 9:39be1525dfe0 127 for(int i = 0; i < 3; i++) {
uchitake 9:39be1525dfe0 128 armMotor[i].setSpeed(0);
uchitake 9:39be1525dfe0 129 }
uchitake 3:369d9ee17e84 130 }
uchitake 1:845af5425eec 131 }
uchitake 1:845af5425eec 132
uchitake 1:845af5425eec 133
uchitake 1:845af5425eec 134 void Bot::calibrate()
uchitake 1:845af5425eec 135 {
uchitake 6:fe9767a50891 136 if(receiveSuccessed &&
uchitake 6:fe9767a50891 137 !pad.getButton2(0) &&
uchitake 6:fe9767a50891 138 !pad.getButton2(1)
uchitake 6:fe9767a50891 139 ) {
uchitake 1:845af5425eec 140 PIDC::calibration(HMC6352_ENTER_CALIB);
uchitake 9:39be1525dfe0 141 quadOmni.moveXY(0, 0, 0.4);
uchitake 3:369d9ee17e84 142 wait(5.0);
uchitake 9:39be1525dfe0 143 quadOmni.moveXY(0, 0, 0);
uchitake 1:845af5425eec 144 PIDC::calibration(HMC6352_EXIT_CALIB);
uchitake 1:845af5425eec 145 }
uchitake 3:369d9ee17e84 146 }