椋 高山 / Mbed 2 deprecated mainboardnrp2018

Dead by Daylight

Dependencies:   mbed Servo

System/Process/Process.cpp@9:49e851ad441e, 2019-02-23 (annotated)

Committer:
takayama_ryo
Date:
Sat Feb 23 02:47:39 2019 +0000
Revision:
9:49e851ad441e
Parent:
8:cb53beff4bb2 
Dead by Daylight

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