NagaokaRoboticsClub_mbedTeam / Mbed OS KANIv3

kani

Dependencies:   2017NHKpin_config FEP omni_wheel PID R1307 ikarashiMDC

classDiagram

　   ＼￣＼　　　　　　　　　　　　　    　 ／￣／ 
/l　    ＼　　＼　　 　　　　　　　　　 ／　　／　lヽ　 
|　ヽ 　ヽ　　 |　　　　 　　　　　　｜　　/　　/　| 
＼　` ‐ヽ　　ヽ　　●　　      ●    　　   /　　/　‐　 ／ 
　 ＼ __　l　　|　　｜|＿＿＿|｜ /　　l　__ ／ 
　　　 　＼　 ＼ ／　　　　　　＼／ 
　　　　　 ／＼|　　 人＿＿人　　|／＼　　　　<ｽﾞﾜｲｶﾞﾆ　 
　　　　／／＼|　　　　　      　　|／＼＼　　　　 
　　　　／／＼|　　　ｹｶﾞﾆ           　|／＼＼　　　　 
　　　　／　  　 . ＼＿＿＿＿＿／　 　      ＼ 

　 　 　 　 　 　 　 　 　 　 　 　    　 　 ┏┓　 　 　 　 ┏━┓┏┓　 　 　 　 　 　 　 
     ┏┓　      　 ┏┓┏┓   ┏┓　 　 ┏┓┗┛   　 ┏┓　┗┓┃┗┛　 　 　 　 　 　 　 
┏┛┗━┓ 　┃┃┃┃    ┃┃┏━┛┗┓┏┓┏┛┗━┓┃┃┏┓┏┓┏━━━┓ 
┗┓┏━┛　 ┃┃┗┛    ┃┃┗━┓┏┛┗┛┗┓┏┓┃┗┛┗┛┃┃┗━━━┛　 　 
┏┛┃┏━┓┃┗━━┓┃┃┏━┛┗┓ 　    ┏┛┃┃┃  　 　 　 ┃┃　 　 　 　 　 　 　 
┃┏┛┗━┛┗━━┓┃┃┃┃┏┓┏┛ 　    ┗━┛┃┃  　 　 　 ┃┃┏┓　 　 　 　 　 
┃┃┏━━┓┏━━┛┃┃┃┃┗┛┃　 　  　 　 ┏┛┃  　 　 　 ┃┃┃┗━━┓　 　 
┗┛┗━━┛┗━━━┛┗┛┗━━┛　 　  　 　 ┗━┛　   　 　 ┗┛┗━━━┛　

bot/bot.cpp@55:f9e13797024f, 2017-12-22 (annotated)

Committer:
takeuchi
Date:
Fri Dec 22 17:34:28 2017 +0900
Revision:
55:f9e13797024f
Parent:
54:be0b5d603562 
reflesh code

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 15:9aa11febe517 4 pad(XBee2TX, XBee2RX, ADDR),
takeuchi 52:01ae5b62a387 5 RS485(MDTX, MDRX, 115200),
uchitake 9:39be1525dfe0 6 RS485Controller(PWM1),
UCHITAKE 15:9aa11febe517 7 powerSwitch(MDstop),
uchitake 9:39be1525dfe0 8 quadOmni(&RS485Controller, &RS485),
uchitake 9:39be1525dfe0 9 slider(&RS485Controller, &RS485),
uchitake 9:39be1525dfe0 10 armMotor({
takeuchi 37:6b6616008e78 11 ikarashiMDC({&RS485Controller, 1, 1, SM, &RS485}),
takeuchi 37:6b6616008e78 12 ikarashiMDC({&RS485Controller, 1, 2, SM, &RS485}),
takeuchi 37:6b6616008e78 13 ikarashiMDC({&RS485Controller, 1, 3, SM, &RS485})
takeuchi 37:6b6616008e78 14 }),
takeuchi 47:43f55ff8916b 15 plane(),
takeuchi 47:43f55ff8916b 16 axis(),
uchitake 9:39be1525dfe0 17 receiveSuccessed(0),
uchitake 28:676330f1d186 18 frontDegree(0),
uchitake 14:1fadf7d2f583 19 led({DebugLED3, DebugLED4, DebugLED5}),
takeuchi 51:794f160b09d4 20 debugSerial(USBTX, USBRX, 115200),
takeuchi 54:be0b5d603562 21 faceMode(0)
uchitake 1:845af5425eec 22 {
uchitake 9:39be1525dfe0 23 for(int i = 0; i < 3; i++) {
uchitake 9:39be1525dfe0 24 armMotor[i].braking = true;
uchitake 9:39be1525dfe0 25 }
uchitake 9:39be1525dfe0 26
uchitake 9:39be1525dfe0 27 quadOmni.moveXY(0, 0, 0);
uchitake 9:39be1525dfe0 28
UCHITAKE 15:9aa11febe517 29 powerSwitch = true;
uchitake 14:1fadf7d2f583 30 for(int i = 0; i < 3; i++) {
uchitake 14:1fadf7d2f583 31 led[i] = true;
uchitake 9:39be1525dfe0 32 wait(0.1);
uchitake 14:1fadf7d2f583 33 led[i] = false;
UCHITAKE 10:99dc4ae08998 34 }
uchitake 1:845af5425eec 35 }
uchitake 1:845af5425eec 36
uchitake 1:845af5425eec 37 void Bot::confirmAll()
uchitake 1:845af5425eec 38 {
uchitake 6:fe9767a50891 39 receiveSuccessed = pad.receiveState();
takeuchi 50:ece1fdc7c9a6 40 if(!pad.getButton2(0) && !pad.getButton2(1) && !pad.getButton2(2) && !pad.getButton2(3)) {
uchitake 16:50651ff960b9 41 powerSwitch = 0;
uchitake 16:50651ff960b9 42 }
uchitake 14:1fadf7d2f583 43 led[0] = receiveSuccessed;
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 14:1fadf7d2f583 56 led[1] = !led[1];
uchitake 3:369d9ee17e84 57 if(pad.getNorm(1) > 0.5) {
takeuchi 47:43f55ff8916b 58 plane.setPoint((pad.getRadian(1) - M_PI / 2) * (180.0 / M_PI));
takeuchi 32:b619c7787dc3 59 plane.confirm();
uchitake 3:369d9ee17e84 60 }
uchitake 9:39be1525dfe0 61 quadOmni.moveXY(
takeuchi 37:6b6616008e78 62 pad.getStick(0),
takeuchi 37:6b6616008e78 63 -pad.getStick(1),
takeuchi 37:6b6616008e78 64 0,
takeuchi 37:6b6616008e78 65 0,
takeuchi 55:f9e13797024f 66 -pad.getStick(2) / 3.0
takeuchi 37:6b6616008e78 67 );
uchitake 3:369d9ee17e84 68 } else {
uchitake 9:39be1525dfe0 69 quadOmni.moveXY(0, 0, 0);
uchitake 3:369d9ee17e84 70 }
uchitake 3:369d9ee17e84 71 }
uchitake 3:369d9ee17e84 72
uchitake 3:369d9ee17e84 73 void Bot::controllDrive2()
uchitake 3:369d9ee17e84 74 {
takeuchi 32:b619c7787dc3 75 plane.confirm();
takeuchi 32:b619c7787dc3 76 axis.confirm();
uchitake 4:1073deb368df 77 float moment = 0;
takeuchi 45:a3ff2bc0574b 78 float norm = 0;
uchitake 3:369d9ee17e84 79 static float beforestick = pad.getStick(2);
uchitake 6:fe9767a50891 80
takeuchi 44:3466b8e98fd9 81 if(!pad.getButton2(4)) {
takeuchi 32:b619c7787dc3 82 axis.resetOffset();
uchitake 5:16ea97725085 83 }
uchitake 3:369d9ee17e84 84
takeuchi 45:a3ff2bc0574b 85 if((beforestick >= 0.2 && pad.getStick(2) < 0.2) || (beforestick <= -0.2 && pad.getStick(2) > -0.2)) {
takeuchi 47:43f55ff8916b 86 plane.setPoint(0.0);
takeuchi 32:b619c7787dc3 87 plane.resetOffset();
uchitake 3:369d9ee17e84 88 }
uchitake 3:369d9ee17e84 89
takeuchi 45:a3ff2bc0574b 90 if(pad.getStick(2) > 0.2 || pad.getStick(2) < -0.2) {
takeuchi 53:701d5c4571e3 91 moment = pad.getStick(2) / 2.5;
uchitake 29:41f6fc4c8962 92 } else {
takeuchi 32:b619c7787dc3 93 moment = plane.getCalculationResult();
uchitake 3:369d9ee17e84 94 }
takeuchi 50:ece1fdc7c9a6 95 if(pad.getButton2(0)) {
takeuchi 45:a3ff2bc0574b 96 norm = pad.getNorm(0) / 2.0;
takeuchi 45:a3ff2bc0574b 97 } else {
takeuchi 45:a3ff2bc0574b 98 norm = pad.getNorm(0);
takeuchi 45:a3ff2bc0574b 99 }
uchitake 6:fe9767a50891 100 if(receiveSuccessed) {
uchitake 14:1fadf7d2f583 101 led[1] = !led[1];
uchitake 9:39be1525dfe0 102 quadOmni.moveCircular(
takeuchi 45:a3ff2bc0574b 103 norm,
takeuchi 53:701d5c4571e3 104 pad.getRadian(0) - axis.getCurrentDegree() /1.0 * (M_PI / 180.0) + (M_PI / 2.0) * faceMode,
takeuchi 39:07180d39a030 105 0.0,
takeuchi 39:07180d39a030 106 0.0,
takeuchi 37:6b6616008e78 107 -moment
takeuchi 37:6b6616008e78 108 );
uchitake 3:369d9ee17e84 109 } else {
uchitake 9:39be1525dfe0 110 quadOmni.moveXY(0, 0, 0);
uchitake 3:369d9ee17e84 111 }
uchitake 3:369d9ee17e84 112 beforestick = pad.getStick(2);
uchitake 1:845af5425eec 113 }
uchitake 1:845af5425eec 114
uchitake 17:79fa65706f92 115 void Bot::controllDrive3()
uchitake 17:79fa65706f92 116 {
uchitake 28:676330f1d186 117 static int rollR = 0;
uchitake 28:676330f1d186 118 static int rollL = 0;
uchitake 17:79fa65706f92 119 static int mode = 1;
uchitake 17:79fa65706f92 120 if(receiveSuccessed) {
uchitake 28:676330f1d186 121 if(rollR && !pad.getButton2(2)) {
uchitake 28:676330f1d186 122 frontDegree += ADJUST_DEGREE;
uchitake 28:676330f1d186 123 }
uchitake 28:676330f1d186 124 rollR = pad.getButton2(2);
uchitake 28:676330f1d186 125
uchitake 28:676330f1d186 126 if(rollL && !pad.getButton2(0)) {
uchitake 28:676330f1d186 127 frontDegree -= ADJUST_DEGREE;
uchitake 28:676330f1d186 128 }
uchitake 28:676330f1d186 129 rollL = pad.getButton2(0);
uchitake 17:79fa65706f92 130
uchitake 17:79fa65706f92 131 if(!pad.getButton2(4)) {
uchitake 17:79fa65706f92 132 mode = 1;
uchitake 17:79fa65706f92 133 }
uchitake 17:79fa65706f92 134
uchitake 17:79fa65706f92 135 if(!pad.getButton2(5)) {
uchitake 17:79fa65706f92 136 mode = 2;
uchitake 17:79fa65706f92 137 }
uchitake 17:79fa65706f92 138
uchitake 17:79fa65706f92 139 if(mode == 1) {
takeuchi 32:b619c7787dc3 140 plane.setPoint(frontDegree);
takeuchi 32:b619c7787dc3 141 plane.confirm();
uchitake 17:79fa65706f92 142
uchitake 17:79fa65706f92 143 quadOmni.moveXY(
takeuchi 37:6b6616008e78 144 pad.getStick(0),
takeuchi 37:6b6616008e78 145 -pad.getStick(1),
takeuchi 37:6b6616008e78 146 0.5,
takeuchi 37:6b6616008e78 147 0.5,
takeuchi 37:6b6616008e78 148 -plane.getCalculationResult()
takeuchi 37:6b6616008e78 149 );
uchitake 17:79fa65706f92 150 }
uchitake 17:79fa65706f92 151 if(mode == 2) {
takeuchi 32:b619c7787dc3 152 plane.setPoint(90.0 + frontDegree);
takeuchi 32:b619c7787dc3 153 plane.confirm();
uchitake 17:79fa65706f92 154
uchitake 17:79fa65706f92 155 quadOmni.moveXY(
takeuchi 37:6b6616008e78 156 -pad.getStick(1),
takeuchi 37:6b6616008e78 157 -pad.getStick(0),
takeuchi 37:6b6616008e78 158 0.5,
takeuchi 37:6b6616008e78 159 0.5,
takeuchi 37:6b6616008e78 160 -plane.getCalculationResult()
takeuchi 37:6b6616008e78 161 );
uchitake 17:79fa65706f92 162 }
uchitake 17:79fa65706f92 163 } else {
uchitake 17:79fa65706f92 164 quadOmni.moveXY(0, 0, 0);
uchitake 17:79fa65706f92 165 }
uchitake 17:79fa65706f92 166 }
uchitake 17:79fa65706f92 167
takeuchi 41:ae6f844facb1 168 void Bot::controllDrive4()
takeuchi 41:ae6f844facb1 169 {
takeuchi 41:ae6f844facb1 170 if(receiveSuccessed) {
takeuchi 41:ae6f844facb1 171 quadOmni.moveXY(
takeuchi 41:ae6f844facb1 172 pad.getStick(0),
takeuchi 41:ae6f844facb1 173 pad.getStick(1),
takeuchi 41:ae6f844facb1 174 0.0,
takeuchi 41:ae6f844facb1 175 0.0,
takeuchi 41:ae6f844facb1 176 -pad.getStick(2)/2.0
takeuchi 41:ae6f844facb1 177 );
takeuchi 41:ae6f844facb1 178 } else {
takeuchi 41:ae6f844facb1 179 quadOmni.moveXY(0, 0, 0);
takeuchi 41:ae6f844facb1 180 }
takeuchi 41:ae6f844facb1 181 }
uchitake 1:845af5425eec 182 void Bot::controllMech()
uchitake 1:845af5425eec 183 {
uchitake 6:fe9767a50891 184 if(receiveSuccessed) {
takeuchi 36:c1398ea8f604 185 if(!pad.getButton1(2)) {
takeuchi 36:c1398ea8f604 186 armMotor[ROLL].setSpeed(1.0);
takeuchi 36:c1398ea8f604 187 } else if(!pad.getButton1(4)) {
takeuchi 36:c1398ea8f604 188 armMotor[ROLL].setSpeed(-1.0);
takeuchi 36:c1398ea8f604 189 }
takeuchi 36:c1398ea8f604 190 if(pad.getButton1(2) && pad.getButton1(4)) {
takeuchi 36:c1398ea8f604 191 armMotor[ROLL].setSpeed(0.0);
takeuchi 36:c1398ea8f604 192 }
takeuchi 36:c1398ea8f604 193
takeuchi 44:3466b8e98fd9 194 if(!pad.getButton2(3)) {
takeuchi 37:6b6616008e78 195 armMotor[SWORD].setSpeed(1.0);
uchitake 16:50651ff960b9 196 }
takeuchi 45:a3ff2bc0574b 197 if(!pad.getButton2(2)) {
takeuchi 45:a3ff2bc0574b 198 armMotor[SWORD].setSpeed(-1.0);
takeuchi 45:a3ff2bc0574b 199 }
takeuchi 45:a3ff2bc0574b 200 if(pad.getButton2(3) && pad.getButton2(2)) armMotor[SWORD].setSpeed(0.0);
uchitake 1:845af5425eec 201
takeuchi 47:43f55ff8916b 202 slider.slide(pad.getStick(3));
UCHITAKE 15:9aa11febe517 203
uchitake 16:50651ff960b9 204 if(!pad.getButton2(1)) {
UCHITAKE 15:9aa11febe517 205 armMotor[DESTROY].setSpeed(-1.0);
takeuchi 54:be0b5d603562 206 } else if(!pad.getButton1(0)) {
takeuchi 54:be0b5d603562 207 armMotor[DESTROY].setSpeed(1.0);
UCHITAKE 15:9aa11febe517 208 } else {
UCHITAKE 15:9aa11febe517 209 armMotor[DESTROY].setSpeed(0.0);
UCHITAKE 15:9aa11febe517 210 }
takeuchi 37:6b6616008e78 211 } else {
takeuchi 37:6b6616008e78 212 slider.slide(0);
takeuchi 37:6b6616008e78 213 for(int i = 0; i < 3; i++) {
takeuchi 37:6b6616008e78 214 armMotor[i].setSpeed(0);
takeuchi 37:6b6616008e78 215 }
takeuchi 37:6b6616008e78 216 }
takeuchi 51:794f160b09d4 217 if(!pad.getButton1(3)) faceMode = 2;
takeuchi 51:794f160b09d4 218 if(!pad.getButton1(5)) faceMode = 0;
uchitake 1:845af5425eec 219 }
uchitake 1:845af5425eec 220
uchitake 1:845af5425eec 221
uchitake 1:845af5425eec 222 void Bot::calibrate()
uchitake 1:845af5425eec 223 {
uchitake 3:369d9ee17e84 224 }