Taiki Maruyama
kaahddhgfhgfghgf
Embed:
(wiki syntax)
Show/hide line numbers
Process.cpp
00001 //2018/02/24のやつ 00002 00003 #include "Process.h" 00004 00005 #include "mbed.h" 00006 #include "../../Communication/XBee/XBee.h" 00007 #include "../../Input/Switch/Switch.h" 00008 #include "../../Output/Motor/Motor.h" 00009 #include "../../Output/Servo/Servo.h" 00010 00011 //_____________________ 00012 /*---------------- HOW TO WRITE ----------------/ 00013 00014 ・motor の割り当てを決める 00015 #define TIRE_L 1 00016 00017 ・リミットスイッチの割り当てを決める 00018 #define 0_L 1 00019 00020 ・他にも自由に定義してもいいです (pwmとか) 00021 00022 /---------------- HOW TO WRITE ----------------*/ 00023 //↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓ 00024 00025 #define TIRE_LF 00026 #define TIRE_LB 1 00027 #define TIRE_RB 2 00028 #define TIRE_RF 3 00029 #define ARM_M 4 00030 00031 #define Air1_pin A7 00032 #define Air2_pin A6 00033 #define Air3_pin A5 00034 #define ARM_T 3 00035 00036 00037 00038 00039 const int omni[15][15]= { 00040 { 0, 2, 6, 15, 26, 41, 59, 80, 80, 80, 80, 80, 80, 80, 80}, 00041 { -2, 0, 2, 6, 15, 26, 41, 59, 61, 65, 73, 80, 80, 80, 80}, 00042 { -6, -2, 0, 2, 6, 15, 26, 41, 43, 47, 56, 67, 80, 80, 80}, 00043 { -15, -6, -2, 0, 2, 6, 15, 26, 28, 33, 41, 52, 67, 80, 80}, 00044 { -26, -15, -6, -2, 0, 2, 6, 15, 17, 21, 29, 41, 56, 73, 80}, 00045 { -41, -26, -15, -6, -2, 0, 2, 6, 8, 13, 21, 33, 48, 65, 80}, 00046 { -59, -41, -26, -15, -6, -2, 0, 2, 4, 8, 16, 28, 42, 61, 80}, 00047 { -80, -59, -41, -26, -15, -6, -2, 0, 2, 6, 15, 26, 41, 59, 80}, 00048 { -80, -61, -42, -28, -16, -8, -4, -2, 0, 2, 6, 15, 26, 41, 59}, 00049 { -80, -65, -48, -33, -21, -13, -8, -6, -2, 0, 2, 6, 15, 26, 41}, 00050 { -80, -73, -56, -41, -29, -21, -17, -15, -6, -2, 0, 2, 6, 15, 26}, 00051 { -80, -80, -67, -52, -41, -33, -28, -26, -15, -6, -2, 0, 2, 6, 15}, 00052 { -80, -80, -80, -67, -56, -47, -43, -41, -26, -15, -6, -2, 0, 2, 6}, 00053 { -80, -80, -80, -80, -73, -65, -61, -59, -41, -26, -15, -6, -2, 0, 2}, 00054 { -80, -80, -80, -80, -80, -80, -80, -80, -59, -41, -26, -15, -6, -2, 0} 00055 }; 00056 00057 const int curve[15] = {-64, -47, -33, -21, -12, -5, -1, 0, 1, 5, 12, 21, 33, 47, 64}; 00058 00059 00060 00061 uint8_t SetStatus(int); 00062 uint8_t SetStatus(int pwmVal) 00063 { 00064 if(pwmVal < 0) return BACK; 00065 else if(pwmVal > 0) return FOR; 00066 else if(pwmVal == 0) return BRAKE; 00067 } 00068 uint8_t SetPWM(int); 00069 uint8_t SetPWM(int pwmVal) 00070 { 00071 if(pwmVal == 0 || pwmVal > 80 || pwmVal < -80) return 80; 00072 else return abs(pwmVal); 00073 } 00074 00075 //↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑ 00076 //_____________________ 00077 00078 //#define USE_USB_SERIAL 00079 #ifdef USE_USB_SERIAL 00080 Serial pc(SERIAL_TX, SERIAL_RX); 00081 #endif 00082 XBEE::ControllerData *controller; 00083 MOTOR::MotorStatus motor[MOUNTING_MOTOR_NUM]; 00084 00085 00086 00087 using namespace SWITCH; 00088 ///////////////////////////////// 00089 DigitalOut AirPin[] = { 00090 DigitalOut(Air1_pin), 00091 DigitalOut(Air2_pin), 00092 DigitalOut(Air3_pin), 00093 }; 00094 00095 void AirOut(int pin,int mode) 00096 { 00097 if(mode==1) { 00098 AirPin[pin]=1; 00099 } else if(mode==0) { 00100 AirPin[pin]=0; 00101 } else { 00102 AirPin[pin]=0; 00103 } 00104 } 00105 ////////////////////////////////// 00106 void SystemProcess(void) 00107 { 00108 while(true) { 00109 controller = XBEE::Controller::GetData(); 00110 //____________________________ 00111 /*------------------------ HOW TO WRITE ------------------------/ 00112 00113 ここにメインのプログラムを書く 00114 00115 ・コントローラから受け取ったデータをもとに動作のプログラムを書く 00116 (コントローラのデータは controller-> で取る) 00117 00118 if(controller->Button.RIGHT) { 00119 motor[TIRE_L].dir = FOR; 00120 motor[TIRE_R].dir = BACK; 00121 motor[TIRE_L].pwm = 12.3; 00122 motor[TIRE_R].pwm = 12.3; 00123 } 00124 00125 motor[0].dirは FOR (正転) 00126 BACK (逆転) 00127 BRAKE (ブレーキ) 00128 FREE (フリー) 00129 00130 motor[0].pwmは 0.0(%) ~ 100.0(%) 00131 00132 controllerは XBee.hの構造体の中身 00133 00134 (AnalogL・Rを使いたかったら、頑張って考える or 聞いてください) 00135 00136 ・リミットスイッチの値をもとに動作のプログラムを書く 00137 00138 if(Switch::CheckPushed(0_L)) 00139 { 00140 if(controller->Button.L) 00141 { 00142 motor[0].dir = FOR; 00143 motor[0].pwm = 80.0; 00144 } 00145 if(motor[0].dir == BACK) 00146 { 00147 motor[0].dir = BRAKE; 00148 } 00149 } 00150 00151 →関数 Switch::CheckPushed の引数はリミットスイッチの名前 (limitSw[0]みたいな), 返り値はbool型 (true or false) 00152 00153 ・他にもやりたいことがあったら自由にどうぞ 00154 00155 ps.わからないことがあったら聞いてください 00156 00157 /------------------------ HOW TO WRITE ------------------------*/ 00158 //↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓ 00159 00160 00161 if(controller->AnalogL.X >= 6 && controller->AnalogL.X <= 8 && controller->AnalogL.Y >= 6 && controller->AnalogL.Y <= 8) { 00162 motor[TIRE_LF].dir = FREE; 00163 motor[TIRE_LB].dir = FREE; 00164 motor[TIRE_RB].dir = FREE; 00165 motor[TIRE_RF].dir = FREE; 00166 } 00167 00168 if(controller->AnalogR.X >= 6 && controller->AnalogR.X <= 8 && controller->AnalogR.Y >= 6 && controller->AnalogR.Y <= 8) { 00169 motor[TIRE_LF].dir = FREE; 00170 motor[TIRE_LB].dir = FREE; 00171 motor[TIRE_RB].dir = FREE; 00172 motor[TIRE_RF].dir = FREE; 00173 } 00174 00175 motor[TIRE_RF].dir = SetStatus(-omni[controller->AnalogL.Y][14-controller->AnalogL.X] + curve[controller->AnalogR.X]); 00176 motor[TIRE_LF].dir = SetStatus(omni[controller->AnalogL.Y][controller->AnalogL.X] + curve[controller->AnalogR.X]); 00177 motor[TIRE_RB].dir = SetStatus(-omni[14-controller->AnalogL.X][14-controller->AnalogL.Y] + curve[controller->AnalogR.X]); 00178 motor[TIRE_LB].dir = SetStatus(omni[controller->AnalogL.X][14-controller->AnalogL.Y] + curve[controller->AnalogR.X]); 00179 00180 motor[TIRE_RF].pwm = SetPWM(omni[controller->AnalogL.Y][14-controller->AnalogL.X]) ; 00181 motor[TIRE_LF].pwm = SetPWM(omni[controller->AnalogL.Y][controller->AnalogL.X]) ; 00182 motor[TIRE_RB].pwm = SetPWM(omni[14-controller->AnalogL.X][14-controller->AnalogL.Y]) ; 00183 motor[TIRE_LB].pwm = SetPWM(omni[controller->AnalogL.X][14-controller->AnalogL.Y]) ; 00184 00185 if(controller->Button.R) { 00186 AirOut(0,1); 00187 } 00188 if(controller->Button.L) { 00189 AirOut(0,0); 00190 } 00191 00192 if(controller->Button.A) { 00193 AirOut(1,1); 00194 } 00195 if(controller->Button.B) { 00196 AirOut(1,0); 00197 } 00198 00199 if(controller->Button.X) { 00200 AirOut(2,1); 00201 } 00202 if(controller->Button.Y) { 00203 AirOut(2,0); 00204 } 00205 00206 if(controller->Button.UP) { 00207 if(Switch::checkPushed(ARM_T)){ 00208 motor[ARM_M].dir = BRAKE; 00209 motor[ARM_M].pwm = 50; 00210 }else{ 00211 motor[ARM_M].dir = BACK; 00212 motor[ARM_M].pwm = 50; 00213 } 00214 }else if(controller->Button.DOWN) { 00215 motor[ARM_M].dir = FOR; 00216 motor[ARM_M].pwm = 50; 00217 }else { 00218 motor[ARM_M].dir = FREE; 00219 } 00220 00221 00222 00223 00224 00225 00226 //↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑ 00227 //____________________________ 00228 00229 MOTOR::Motor::Update(motor); 00230 } 00231 } 00232
Generated on Mon Jul 18 2022 07:48:18 by
1.7.2