Taiki Maruyama / Mbed 2 deprecated charyaaaaaaaaa

kaahddhgfhgfghgf

Dependencies:   mbed Servo

System/Process/Process.cpp@10:a81546661fb5, 2019-02-23 (annotated)

Committer:
kishibekairohan
Date:
Sat Feb 23 05:34:18 2019 +0000
Revision:
10:a81546661fb5
Parent:
9:49e851ad441e
Child:
11:de0dc9fba6c9
aaaa;

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