NagaokaRoboticsClub_mbedTeam / Mbed OS NHK2017_octopus2_drive

タコ　駆動側　

Dependencies:   2017NHKpin_config FEP R1307 PID ikarashiMDC omni_wheel

Fork of NHK2017_octopus2 by NagaokaRoboticsClub_mbedTeam

bot/bot.cpp@23:797d25f3df5e, 2017-10-01 (annotated)

Committer:
takeuchi
Date:
Sun Oct 01 11:43:53 2017 +0900
Branch:
develop1
Revision:
23:797d25f3df5e
Parent:
19:34da005ea4ea 
fix inheritance problem

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() :
takeuchi 23:797d25f3df5e 4 pidc(),
UCHITAKE 15:9aa11febe517 5 pad(XBee2TX, XBee2RX, ADDR),
uchitake 9:39be1525dfe0 6 RS485(MDTX, MDRX, 38400),
uchitake 9:39be1525dfe0 7 RS485Controller(PWM1),
UCHITAKE 15:9aa11febe517 8 powerSwitch(MDstop),
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 15:9aa11febe517 26 powerSwitch = true;
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 16:50651ff960b9 37 if(!pad.getButton2(3) && !pad.getButton2(0)) {
uchitake 16:50651ff960b9 38 powerSwitch = 0;
uchitake 16:50651ff960b9 39 }
uchitake 14:1fadf7d2f583 40 led[0] = receiveSuccessed;
uchitake 6:fe9767a50891 41 if(!receiveSuccessed) {
uchitake 9:39be1525dfe0 42 quadOmni.moveXY(0, 0, 0);
uchitake 9:39be1525dfe0 43 slider.slide(0);
uchitake 9:39be1525dfe0 44 for(int i = 0; i < 3; i++) {
uchitake 9:39be1525dfe0 45 armMotor[i].setSpeed(0);
uchitake 9:39be1525dfe0 46 }
uchitake 1:845af5425eec 47 }
uchitake 1:845af5425eec 48 }
uchitake 1:845af5425eec 49
uchitake 1:845af5425eec 50 void Bot::controllDrive()
uchitake 1:845af5425eec 51 {
uchitake 17:79fa65706f92 52 static int mode = 1;
uchitake 17:79fa65706f92 53 if(receiveSuccessed) {
takeuchi 23:797d25f3df5e 54 debugSerial.printf("%d\n\r", pidc.getCurrentDegree());
uchitake 17:79fa65706f92 55
uchitake 17:79fa65706f92 56 if(!pad.getButton2(4)) {
uchitake 17:79fa65706f92 57 mode = 1;
uchitake 17:79fa65706f92 58 }
uchitake 17:79fa65706f92 59
uchitake 17:79fa65706f92 60 if(!pad.getButton2(5)) {
uchitake 17:79fa65706f92 61 mode = 2;
uchitake 17:79fa65706f92 62 }
uchitake 17:79fa65706f92 63
uchitake 17:79fa65706f92 64 if(mode == 1) {
takeuchi 23:797d25f3df5e 65 pidc.setPoint(0.0);
takeuchi 23:797d25f3df5e 66 pidc.confirm();
uchitake 17:79fa65706f92 67
uchitake 17:79fa65706f92 68 quadOmni.moveXY(
uchitake 17:79fa65706f92 69 pad.getStick(0),
uchitake 17:79fa65706f92 70 -pad.getStick(1),
takeuchi 23:797d25f3df5e 71 -pidc.getCalculationResult()
uchitake 17:79fa65706f92 72 );
uchitake 17:79fa65706f92 73 }
uchitake 17:79fa65706f92 74 if(mode == 2) {
takeuchi 23:797d25f3df5e 75 pidc.setPoint(90.0);
takeuchi 23:797d25f3df5e 76 pidc.confirm();
uchitake 17:79fa65706f92 77
uchitake 17:79fa65706f92 78 quadOmni.moveXY(
uchitake 17:79fa65706f92 79 -pad.getStick(1),
uchitake 19:34da005ea4ea 80 -pad.getStick(0),
takeuchi 23:797d25f3df5e 81 -pidc.getCalculationResult()
uchitake 17:79fa65706f92 82 );
uchitake 17:79fa65706f92 83 }
uchitake 17:79fa65706f92 84 } else {
uchitake 17:79fa65706f92 85 quadOmni.moveXY(0, 0, 0);
uchitake 17:79fa65706f92 86 }
uchitake 17:79fa65706f92 87 }
uchitake 17:79fa65706f92 88
uchitake 1:845af5425eec 89 void Bot::controllMech()
uchitake 1:845af5425eec 90 {
uchitake 6:fe9767a50891 91 if(receiveSuccessed) {
uchitake 16:50651ff960b9 92 if(!pad.getButton1(6)) {
uchitake 16:50651ff960b9 93 armMotor[SWORD].setSpeed(0.6);
uchitake 16:50651ff960b9 94 }
uchitake 16:50651ff960b9 95 if(pad.getButton1(6)) armMotor[SWORD].setSpeed(0.0);
uchitake 1:845af5425eec 96
uchitake 19:34da005ea4ea 97 slider.slide(pad.getStick(3));
UCHITAKE 15:9aa11febe517 98
uchitake 16:50651ff960b9 99 if(!pad.getButton2(1)) {
UCHITAKE 15:9aa11febe517 100 armMotor[DESTROY].setSpeed(-1.0);
UCHITAKE 15:9aa11febe517 101 } else {
UCHITAKE 15:9aa11febe517 102 armMotor[DESTROY].setSpeed(0.0);
UCHITAKE 15:9aa11febe517 103 }
uchitake 3:369d9ee17e84 104 } else {
uchitake 9:39be1525dfe0 105 slider.slide(0);
uchitake 9:39be1525dfe0 106 for(int i = 0; i < 3; i++) {
uchitake 9:39be1525dfe0 107 armMotor[i].setSpeed(0);
uchitake 9:39be1525dfe0 108 }
uchitake 3:369d9ee17e84 109 }
uchitake 1:845af5425eec 110 }
uchitake 1:845af5425eec 111
uchitake 1:845af5425eec 112
uchitake 1:845af5425eec 113 void Bot::calibrate()
uchitake 1:845af5425eec 114 {
uchitake 6:fe9767a50891 115 if(receiveSuccessed &&
uchitake 6:fe9767a50891 116 !pad.getButton2(0) &&
uchitake 6:fe9767a50891 117 !pad.getButton2(1)
uchitake 6:fe9767a50891 118 ) {
uchitake 19:34da005ea4ea 119 t.start();
uchitake 19:34da005ea4ea 120 t.reset();
takeuchi 23:797d25f3df5e 121 pidc.calibration(HMC6352_ENTER_CALIB);
uchitake 19:34da005ea4ea 122 while(t.read() < 5.0) {
uchitake 19:34da005ea4ea 123 quadOmni.moveXY(0, 0, 0.4);
uchitake 19:34da005ea4ea 124 slider.slide(0);
uchitake 19:34da005ea4ea 125 for(int i = 0; i < 3; i++) {
uchitake 19:34da005ea4ea 126 armMotor[i].setSpeed(0);
uchitake 19:34da005ea4ea 127 }
uchitake 19:34da005ea4ea 128 }
uchitake 19:34da005ea4ea 129 t.stop();
uchitake 9:39be1525dfe0 130 quadOmni.moveXY(0, 0, 0);
takeuchi 23:797d25f3df5e 131 pidc.calibration(HMC6352_EXIT_CALIB);
uchitake 1:845af5425eec 132 }
uchitake 3:369d9ee17e84 133 }