Taiki Maruyama / Mbed 2 deprecated charyaaaaaaaaa

kaahddhgfhgfghgf

Dependencies:   mbed Servo

System/Process/Process.cpp@12:419fc52505ee, 2019-02-23 (annotated)

Committer:
kishibekairohan
Date:
Sat Feb 23 05:40:34 2019 +0000
Revision:
12:419fc52505ee
Parent:
11:de0dc9fba6c9 
llll

Who changed what in which revision?

UserRevisionLine numberNew contents of line
t_yamamoto 8:cb53beff4bb2 1 //2018/02/24のやつ
t_yamamoto 5:a802ab60df1b 2
t_yamamoto 0:562021ed1ba9 3 #include "Process.h"
t_yamamoto 0:562021ed1ba9 4
t_yamamoto 0:562021ed1ba9 5 #include "mbed.h"
t_yamamoto 0:562021ed1ba9 6 #include "../../Communication/XBee/XBee.h"
t_yamamoto 0:562021ed1ba9 7 #include "../../Input/Switch/Switch.h"
t_yamamoto 0:562021ed1ba9 8 #include "../../Output/Motor/Motor.h"
t_yamamoto 8:cb53beff4bb2 9 #include "../../Output/Servo/Servo.h"
t_yamamoto 8:cb53beff4bb2 10
t_yamamoto 0:562021ed1ba9 11 //_____________________
t_yamamoto 0:562021ed1ba9 12 /*---------------- HOW TO WRITE ----------------/
t_yamamoto 0:562021ed1ba9 13
t_yamamoto 0:562021ed1ba9 14 ・motor の割り当てを決める
t_yamamoto 0:562021ed1ba9 15 #define TIRE_L 1
t_yamamoto 0:562021ed1ba9 16
t_yamamoto 0:562021ed1ba9 17 ・リミットスイッチの割り当てを決める
takayama_ryo 9:49e851ad441e 18 #define 0_L 1
t_yamamoto 0:562021ed1ba9 19
t_yamamoto 0:562021ed1ba9 20 ・他にも自由に定義してもいいです (pwmとか)
t_yamamoto 0:562021ed1ba9 21
t_yamamoto 0:562021ed1ba9 22 /---------------- HOW TO WRITE ----------------*/
t_yamamoto 0:562021ed1ba9 23 //↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓
t_yamamoto 0:562021ed1ba9 24
kishibekairohan 10:a81546661fb5 25 #define TIRE_LF
takayama_ryo 9:49e851ad441e 26 #define TIRE_LB 1
takayama_ryo 9:49e851ad441e 27 #define TIRE_RB 2
takayama_ryo 9:49e851ad441e 28 #define TIRE_RF 3
takayama_ryo 9:49e851ad441e 29 #define ARM_M 4
takayama_ryo 9:49e851ad441e 30
takayama_ryo 9:49e851ad441e 31 #define Air1_pin A7
takayama_ryo 9:49e851ad441e 32 #define Air2_pin A6
takayama_ryo 9:49e851ad441e 33 #define Air3_pin A5
kishibekairohan 10:a81546661fb5 34 #define ARM_T 3
takayama_ryo 9:49e851ad441e 35
kishibekairohan 11:de0dc9fba6c9 36
kishibekairohan 12:419fc52505ee 37
kishibekairohan 12:419fc52505ee 38
kishibekairohan 10:a81546661fb5 39 const int omni[15][15]= {
takayama_ryo 9:49e851ad441e 40 { 0, 2, 6, 15, 26, 41, 59, 80, 80, 80, 80, 80, 80, 80, 80},
takayama_ryo 9:49e851ad441e 41 { -2, 0, 2, 6, 15, 26, 41, 59, 61, 65, 73, 80, 80, 80, 80},
takayama_ryo 9:49e851ad441e 42 { -6, -2, 0, 2, 6, 15, 26, 41, 43, 47, 56, 67, 80, 80, 80},
takayama_ryo 9:49e851ad441e 43 { -15, -6, -2, 0, 2, 6, 15, 26, 28, 33, 41, 52, 67, 80, 80},
takayama_ryo 9:49e851ad441e 44 { -26, -15, -6, -2, 0, 2, 6, 15, 17, 21, 29, 41, 56, 73, 80},
takayama_ryo 9:49e851ad441e 45 { -41, -26, -15, -6, -2, 0, 2, 6, 8, 13, 21, 33, 48, 65, 80},
takayama_ryo 9:49e851ad441e 46 { -59, -41, -26, -15, -6, -2, 0, 2, 4, 8, 16, 28, 42, 61, 80},
takayama_ryo 9:49e851ad441e 47 { -80, -59, -41, -26, -15, -6, -2, 0, 2, 6, 15, 26, 41, 59, 80},
takayama_ryo 9:49e851ad441e 48 { -80, -61, -42, -28, -16, -8, -4, -2, 0, 2, 6, 15, 26, 41, 59},
takayama_ryo 9:49e851ad441e 49 { -80, -65, -48, -33, -21, -13, -8, -6, -2, 0, 2, 6, 15, 26, 41},
takayama_ryo 9:49e851ad441e 50 { -80, -73, -56, -41, -29, -21, -17, -15, -6, -2, 0, 2, 6, 15, 26},
takayama_ryo 9:49e851ad441e 51 { -80, -80, -67, -52, -41, -33, -28, -26, -15, -6, -2, 0, 2, 6, 15},
takayama_ryo 9:49e851ad441e 52 { -80, -80, -80, -67, -56, -47, -43, -41, -26, -15, -6, -2, 0, 2, 6},
takayama_ryo 9:49e851ad441e 53 { -80, -80, -80, -80, -73, -65, -61, -59, -41, -26, -15, -6, -2, 0, 2},
takayama_ryo 9:49e851ad441e 54 { -80, -80, -80, -80, -80, -80, -80, -80, -59, -41, -26, -15, -6, -2, 0}
takayama_ryo 9:49e851ad441e 55 };
takayama_ryo 9:49e851ad441e 56
takayama_ryo 9:49e851ad441e 57 const int curve[15] = {-64, -47, -33, -21, -12, -5, -1, 0, 1, 5, 12, 21, 33, 47, 64};
t_yamamoto 0:562021ed1ba9 58
t_yamamoto 0:562021ed1ba9 59
t_yamamoto 0:562021ed1ba9 60
takayama_ryo 9:49e851ad441e 61 uint8_t SetStatus(int);
kishibekairohan 10:a81546661fb5 62 uint8_t SetStatus(int pwmVal)
kishibekairohan 10:a81546661fb5 63 {
takayama_ryo 9:49e851ad441e 64 if(pwmVal < 0) return BACK;
takayama_ryo 9:49e851ad441e 65 else if(pwmVal > 0) return FOR;
takayama_ryo 9:49e851ad441e 66 else if(pwmVal == 0) return BRAKE;
takayama_ryo 9:49e851ad441e 67 }
takayama_ryo 9:49e851ad441e 68 uint8_t SetPWM(int);
kishibekairohan 10:a81546661fb5 69 uint8_t SetPWM(int pwmVal)
kishibekairohan 10:a81546661fb5 70 {
takayama_ryo 9:49e851ad441e 71 if(pwmVal == 0 || pwmVal > 80 || pwmVal < -80) return 80;
takayama_ryo 9:49e851ad441e 72 else return abs(pwmVal);
takayama_ryo 9:49e851ad441e 73 }
t_yamamoto 0:562021ed1ba9 74
t_yamamoto 0:562021ed1ba9 75 //↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑
t_yamamoto 0:562021ed1ba9 76 //_____________________
t_yamamoto 0:562021ed1ba9 77
t_yamamoto 8:cb53beff4bb2 78 //#define USE_USB_SERIAL
t_yamamoto 0:562021ed1ba9 79 #ifdef USE_USB_SERIAL
t_yamamoto 0:562021ed1ba9 80 Serial pc(SERIAL_TX, SERIAL_RX);
t_yamamoto 0:562021ed1ba9 81 #endif
t_yamamoto 0:562021ed1ba9 82 XBEE::ControllerData *controller;
t_yamamoto 0:562021ed1ba9 83 MOTOR::MotorStatus motor[MOUNTING_MOTOR_NUM];
t_yamamoto 0:562021ed1ba9 84
takayama_ryo 9:49e851ad441e 85
takayama_ryo 9:49e851ad441e 86
t_yamamoto 0:562021ed1ba9 87 using namespace SWITCH;
takayama_ryo 9:49e851ad441e 88 /////////////////////////////////
takayama_ryo 9:49e851ad441e 89 DigitalOut AirPin[] = {
takayama_ryo 9:49e851ad441e 90 DigitalOut(Air1_pin),
takayama_ryo 9:49e851ad441e 91 DigitalOut(Air2_pin),
takayama_ryo 9:49e851ad441e 92 DigitalOut(Air3_pin),
takayama_ryo 9:49e851ad441e 93 };
t_yamamoto 0:562021ed1ba9 94
kishibekairohan 10:a81546661fb5 95 void AirOut(int pin,int mode)
kishibekairohan 10:a81546661fb5 96 {
kishibekairohan 10:a81546661fb5 97 if(mode==1) {
takayama_ryo 9:49e851ad441e 98 AirPin[pin]=1;
kishibekairohan 10:a81546661fb5 99 } else if(mode==0) {
takayama_ryo 9:49e851ad441e 100 AirPin[pin]=0;
kishibekairohan 10:a81546661fb5 101 } else {
kishibekairohan 10:a81546661fb5 102 AirPin[pin]=0;
takayama_ryo 9:49e851ad441e 103 }
takayama_ryo 9:49e851ad441e 104 }
takayama_ryo 9:49e851ad441e 105 //////////////////////////////////
kishibekairohan 10:a81546661fb5 106 void SystemProcess(void)
kishibekairohan 10:a81546661fb5 107 {
t_yamamoto 0:562021ed1ba9 108 while(true) {
t_yamamoto 0:562021ed1ba9 109 controller = XBEE::Controller::GetData();
t_yamamoto 0:562021ed1ba9 110 //____________________________
kishibekairohan 10:a81546661fb5 111 /*------------------------ HOW TO WRITE ------------------------/
t_yamamoto 0:562021ed1ba9 112
kishibekairohan 10:a81546661fb5 113 ここにメインのプログラムを書く
t_yamamoto 0:562021ed1ba9 114
kishibekairohan 10:a81546661fb5 115 ・コントローラから受け取ったデータをもとに動作のプログラムを書く
kishibekairohan 10:a81546661fb5 116 (コントローラのデータは controller-> で取る)
t_yamamoto 0:562021ed1ba9 117
kishibekairohan 10:a81546661fb5 118 if(controller->Button.RIGHT) {
kishibekairohan 10:a81546661fb5 119 motor[TIRE_L].dir = FOR;
kishibekairohan 10:a81546661fb5 120 motor[TIRE_R].dir = BACK;
kishibekairohan 10:a81546661fb5 121 motor[TIRE_L].pwm = 12.3;
kishibekairohan 10:a81546661fb5 122 motor[TIRE_R].pwm = 12.3;
kishibekairohan 10:a81546661fb5 123 }
t_yamamoto 0:562021ed1ba9 124
kishibekairohan 10:a81546661fb5 125 motor[0].dirは FOR (正転)
kishibekairohan 10:a81546661fb5 126 BACK (逆転)
kishibekairohan 10:a81546661fb5 127 BRAKE (ブレーキ)
kishibekairohan 10:a81546661fb5 128 FREE (フリー)
kishibekairohan 10:a81546661fb5 129
kishibekairohan 10:a81546661fb5 130 motor[0].pwmは 0.0(%) ~ 100.0(%)
kishibekairohan 10:a81546661fb5 131
kishibekairohan 10:a81546661fb5 132 controllerは XBee.hの構造体の中身
kishibekairohan 10:a81546661fb5 133
kishibekairohan 10:a81546661fb5 134 (AnalogL・Rを使いたかったら、頑張って考える or 聞いてください)
kishibekairohan 10:a81546661fb5 135
kishibekairohan 10:a81546661fb5 136 ・リミットスイッチの値をもとに動作のプログラムを書く
t_yamamoto 0:562021ed1ba9 137
kishibekairohan 10:a81546661fb5 138 if(Switch::CheckPushed(0_L))
kishibekairohan 10:a81546661fb5 139 {
kishibekairohan 10:a81546661fb5 140 if(controller->Button.L)
kishibekairohan 10:a81546661fb5 141 {
kishibekairohan 10:a81546661fb5 142 motor[0].dir = FOR;
kishibekairohan 10:a81546661fb5 143 motor[0].pwm = 80.0;
kishibekairohan 10:a81546661fb5 144 }
kishibekairohan 10:a81546661fb5 145 if(motor[0].dir == BACK)
kishibekairohan 10:a81546661fb5 146 {
kishibekairohan 10:a81546661fb5 147 motor[0].dir = BRAKE;
kishibekairohan 10:a81546661fb5 148 }
kishibekairohan 10:a81546661fb5 149 }
t_yamamoto 0:562021ed1ba9 150
kishibekairohan 10:a81546661fb5 151 →関数 Switch::CheckPushed の引数はリミットスイッチの名前 (limitSw[0]みたいな), 返り値はbool型 (true or false)
kishibekairohan 10:a81546661fb5 152
kishibekairohan 10:a81546661fb5 153 ・他にもやりたいことがあったら自由にどうぞ
kishibekairohan 10:a81546661fb5 154
kishibekairohan 10:a81546661fb5 155 ps.わからないことがあったら聞いてください
kishibekairohan 10:a81546661fb5 156
kishibekairohan 10:a81546661fb5 157 /------------------------ HOW TO WRITE ------------------------*/
kishibekairohan 10:a81546661fb5 158 //↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓
kishibekairohan 10:a81546661fb5 159
kishibekairohan 10:a81546661fb5 160
kishibekairohan 10:a81546661fb5 161 if(controller->AnalogL.X >= 6 && controller->AnalogL.X <= 8 && controller->AnalogL.Y >= 6 && controller->AnalogL.Y <= 8) {
kishibekairohan 10:a81546661fb5 162 motor[TIRE_LF].dir = FREE;
kishibekairohan 10:a81546661fb5 163 motor[TIRE_LB].dir = FREE;
kishibekairohan 10:a81546661fb5 164 motor[TIRE_RB].dir = FREE;
kishibekairohan 10:a81546661fb5 165 motor[TIRE_RF].dir = FREE;
t_yamamoto 0:562021ed1ba9 166 }
t_yamamoto 0:562021ed1ba9 167
kishibekairohan 10:a81546661fb5 168 if(controller->AnalogR.X >= 6 && controller->AnalogR.X <= 8 && controller->AnalogR.Y >= 6 && controller->AnalogR.Y <= 8) {
kishibekairohan 10:a81546661fb5 169 motor[TIRE_LF].dir = FREE;
kishibekairohan 10:a81546661fb5 170 motor[TIRE_LB].dir = FREE;
kishibekairohan 10:a81546661fb5 171 motor[TIRE_RB].dir = FREE;
kishibekairohan 10:a81546661fb5 172 motor[TIRE_RF].dir = FREE;
kishibekairohan 10:a81546661fb5 173 }
kishibekairohan 10:a81546661fb5 174
kishibekairohan 10:a81546661fb5 175 motor[TIRE_RF].dir = SetStatus(-omni[controller->AnalogL.Y][14-controller->AnalogL.X] + curve[controller->AnalogR.X]);
kishibekairohan 10:a81546661fb5 176 motor[TIRE_LF].dir = SetStatus(omni[controller->AnalogL.Y][controller->AnalogL.X] + curve[controller->AnalogR.X]);
kishibekairohan 10:a81546661fb5 177 motor[TIRE_RB].dir = SetStatus(-omni[14-controller->AnalogL.X][14-controller->AnalogL.Y] + curve[controller->AnalogR.X]);
kishibekairohan 10:a81546661fb5 178 motor[TIRE_LB].dir = SetStatus(omni[controller->AnalogL.X][14-controller->AnalogL.Y] + curve[controller->AnalogR.X]);
t_yamamoto 0:562021ed1ba9 179
kishibekairohan 10:a81546661fb5 180 motor[TIRE_RF].pwm = SetPWM(omni[controller->AnalogL.Y][14-controller->AnalogL.X]) ;
kishibekairohan 10:a81546661fb5 181 motor[TIRE_LF].pwm = SetPWM(omni[controller->AnalogL.Y][controller->AnalogL.X]) ;
kishibekairohan 10:a81546661fb5 182 motor[TIRE_RB].pwm = SetPWM(omni[14-controller->AnalogL.X][14-controller->AnalogL.Y]) ;
kishibekairohan 10:a81546661fb5 183 motor[TIRE_LB].pwm = SetPWM(omni[controller->AnalogL.X][14-controller->AnalogL.Y]) ;
t_yamamoto 0:562021ed1ba9 184
kishibekairohan 10:a81546661fb5 185 if(controller->Button.R) {
kishibekairohan 10:a81546661fb5 186 AirOut(0,1);
kishibekairohan 10:a81546661fb5 187 }
kishibekairohan 10:a81546661fb5 188 if(controller->Button.L) {
kishibekairohan 10:a81546661fb5 189 AirOut(0,0);
kishibekairohan 10:a81546661fb5 190 }
t_yamamoto 0:562021ed1ba9 191
kishibekairohan 10:a81546661fb5 192 if(controller->Button.A) {
kishibekairohan 10:a81546661fb5 193 AirOut(1,1);
kishibekairohan 10:a81546661fb5 194 }
kishibekairohan 10:a81546661fb5 195 if(controller->Button.B) {
kishibekairohan 10:a81546661fb5 196 AirOut(1,0);
kishibekairohan 10:a81546661fb5 197 }
kishibekairohan 10:a81546661fb5 198
kishibekairohan 10:a81546661fb5 199 if(controller->Button.X) {
kishibekairohan 10:a81546661fb5 200 AirOut(2,1);
kishibekairohan 10:a81546661fb5 201 }
kishibekairohan 10:a81546661fb5 202 if(controller->Button.Y) {
kishibekairohan 10:a81546661fb5 203 AirOut(2,0);
kishibekairohan 10:a81546661fb5 204 }
kishibekairohan 10:a81546661fb5 205
kishibekairohan 10:a81546661fb5 206 if(controller->Button.UP) {
kishibekairohan 10:a81546661fb5 207 if(Switch::checkPushed(ARM_T)){
kishibekairohan 10:a81546661fb5 208 motor[ARM_M].dir = BRAKE;
kishibekairohan 10:a81546661fb5 209 motor[ARM_M].pwm = 50;
kishibekairohan 10:a81546661fb5 210 }else{
kishibekairohan 10:a81546661fb5 211 motor[ARM_M].dir = BACK;
kishibekairohan 10:a81546661fb5 212 motor[ARM_M].pwm = 50;
kishibekairohan 10:a81546661fb5 213 }
kishibekairohan 10:a81546661fb5 214 }else if(controller->Button.DOWN) {
kishibekairohan 10:a81546661fb5 215 motor[ARM_M].dir = FOR;
kishibekairohan 10:a81546661fb5 216 motor[ARM_M].pwm = 50;
kishibekairohan 10:a81546661fb5 217 }else {
kishibekairohan 10:a81546661fb5 218 motor[ARM_M].dir = FREE;
kishibekairohan 10:a81546661fb5 219 }
kishibekairohan 10:a81546661fb5 220
kishibekairohan 10:a81546661fb5 221
t_yamamoto 0:562021ed1ba9 222
t_yamamoto 0:562021ed1ba9 223
t_yamamoto 0:562021ed1ba9 224
t_yamamoto 0:562021ed1ba9 225
t_yamamoto 0:562021ed1ba9 226 //↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑
t_yamamoto 0:562021ed1ba9 227 //____________________________
t_yamamoto 0:562021ed1ba9 228
t_yamamoto 0:562021ed1ba9 229 MOTOR::Motor::Update(motor);
t_yamamoto 0:562021ed1ba9 230 }
t_yamamoto 0:562021ed1ba9 231 }
t_yamamoto 0:562021ed1ba9 232