自動機の手動プログラム
Embed:
(wiki syntax)
Show/hide line numbers
Help.cpp
00001 #include "Help.h" 00002 #include "PS3.h" 00003 00004 PS3 ps3(PA_0,PA_1); 00005 I2C i2c(D14,D15); 00006 DigitalOut POWER_LED(D5); 00007 DigitalOut flag(D6); 00008 DigitalIn arm_mae(A3); 00009 DigitalIn arm_usiro(A2); 00010 DigitalIn drop_mae(D3); 00011 DigitalIn drop_usiro(D2); 00012 int motion_phase; 00013 00014 void set_gpio(void) 00015 { 00016 POWER_LED = 1; 00017 arm_mae.mode(PullUp); 00018 arm_usiro.mode(PullUp); 00019 drop_mae.mode(PullUp); 00020 drop_usiro.mode(PullUp); 00021 } 00022 00023 void set_emg(void) 00024 { 00025 bool start = ps3.getSTARTState(); 00026 bool select = ps3.getSELECTState(); 00027 00028 flag = (select == 0 && start == 1)? 0:flag; 00029 flag = (select == 1 && start == 1)? 1:flag; 00030 flag = (select == 1 && start == 0)? 1:flag; 00031 } 00032 00033 void get_stick(int *axis,char mode) 00034 { 00035 switch (mode) { 00036 case 'R': 00037 *axis = ps3.getRightJoystickXaxis(); 00038 ++axis; 00039 *axis = ps3.getRightJoystickYaxis(); 00040 break; 00041 case 'L': 00042 *axis = ps3.getLeftJoystickXaxis(); 00043 ++axis; 00044 *axis = ps3.getLeftJoystickYaxis(); 00045 break; 00046 } 00047 } 00048 00049 bool get_but(int butnum) 00050 { 00051 int ps3_cases[12] = {38,36,16,37,32,33,34,35,19,20,17,18};//sign,cross,rl 00052 return ps3.getButtonState(ps3_cases[butnum]); 00053 } 00054 00055 void set_speed(int *pspeed) 00056 { 00057 *pspeed = (get_but(r2) && get_but(l2))? low:*pspeed; 00058 *pspeed = (ps3.getSTARTState())? fast:*pspeed; 00059 } 00060 00061 void set_direct(int *direct) 00062 { 00063 get_stick(Raxis,'R'); 00064 get_stick(Laxis,'L'); 00065 int Rrad = atan2(double(Raxis[1]), double(Raxis[0]))*double(180/PI); 00066 int Lrad = atan2(double(Laxis[1]), double(Laxis[0]))*double(180/PI); 00067 *direct = SBREAK; 00068 *direct = (Rrad == 0 && Lrad == 0)? 0:*direct; 00069 *direct = (Rrad >= 45 && Rrad <= 135)? 1:*direct; 00070 *direct = (Rrad <= -45 && Rrad >= -135)? 2:*direct; 00071 *direct = (Rrad > -45 && Rrad < 0)||(Rrad > 0 && Rrad < 45)||(Raxis[0] == 63 && Raxis[1] == 0)? 3:*direct; 00072 *direct = (Rrad > 135) || (Rrad > -180 && Rrad < -135)? 4:*direct; 00073 *direct = (Rrad > 0 && get_but(r1))? 5:*direct; 00074 *direct = (Rrad > 0 && get_but(l1))? 6:*direct; 00075 } 00076 00077 void set_tire(char *fmot, char *bmot, int direct, char mode, int speed) 00078 { 00079 int front = SBREAK; 00080 int back = SBREAK; 00081 front = (speed == low)? pDuty50:pDuty70; 00082 back = (speed == low)? nDuty50:nDuty70; 00083 switch (direct) { 00084 case 1: 00085 *fmot = front; 00086 *bmot = front; 00087 break; 00088 case 2: 00089 *fmot = back; 00090 *bmot = back; 00091 break; 00092 case 3: 00093 *fmot = (mode == 'R')? back:front; 00094 *bmot = (mode == 'R')? back:front; 00095 break; 00096 case 4: 00097 *fmot = (mode == 'R')? front:back; 00098 *bmot = (mode == 'R')? front:back; 00099 break; 00100 case 5: 00101 if(speed == low) { 00102 *fmot = (mode == 'R')? front:front+TRANSIT; 00103 *bmot = (mode == 'R')? front:front+TRANSIT; 00104 } else if(speed == fast) { 00105 *fmot = (mode == 'R')? front-TRANSIT:front; 00106 *bmot = (mode == 'R')? front-TRANSIT:front; 00107 } 00108 break; 00109 case 6: 00110 if(speed == low) { 00111 *fmot = (mode == 'L')? front:front+TRANSIT; 00112 *bmot = (mode == 'L')? front:front+TRANSIT; 00113 } else if(speed == fast) { 00114 *fmot = (mode == 'L')? front-TRANSIT:front; 00115 *bmot = (mode == 'L')? front-TRANSIT:front; 00116 } 00117 break; 00118 case 0: 00119 default : 00120 *fmot = SBREAK; 00121 *bmot = SBREAK; 00122 break; 00123 } 00124 } 00125 00126 void set_slide(char *Rfmot,char *Lfmot,char *Rbmot,char *Lbmot,int but1,int but2, int speed) 00127 { 00128 int front = SBREAK; 00129 int back = SBREAK; 00130 front = (speed == low)? pDuty50:pDuty70; 00131 back = (speed == low)? nDuty50:nDuty70; 00132 if(get_but(but1) ^ get_but(but2)) { 00133 int butnum = (get_but(but1) > get_but(but2))? but1:but2; 00134 switch (butnum) { 00135 case r1: 00136 case r2: 00137 *Rfmot = back; 00138 *Lfmot = front; 00139 *Rbmot = front; 00140 *Lbmot = back; 00141 break; 00142 case l1: 00143 case l2: 00144 *Rfmot = front; 00145 *Lfmot = back; 00146 *Rbmot = back; 00147 *Lbmot = front; 00148 break; 00149 } 00150 } else { 00151 *Rfmot = SBREAK; 00152 *Lfmot = SBREAK; 00153 *Rbmot = SBREAK; 00154 *Lbmot = SBREAK; 00155 } 00156 } 00157 00158 void set_mot(char *mot,int num1,int num2) 00159 { 00160 *mot = SBREAK; 00161 if(get_but(num1) != get_but(num2)) { 00162 int butnum = (get_but(num1) > get_but(num2))? num1:num2; 00163 switch (butnum) { 00164 case cir: 00165 *mot = (arm_mae == 1)? pDuty85:SBREAK; 00166 break; 00167 case cro: 00168 *mot = (arm_usiro == 1)? nDuty85:SBREAK; 00169 break; 00170 } 00171 } 00172 } 00173 00174 void set_arm(char *mot, int num, int pulse) 00175 { 00176 //押しっぱなしで動くやつ(まだましさだまさし) 00177 //motion_phase = get_but(num); 00178 00179 //一回ボタン押したら動くやつ(危険) 00180 if(arm_mae == 0 && arm_usiro == 1) { 00181 motion_phase = get_but(num); 00182 } 00183 00184 switch (motion_phase) { 00185 case 0: 00186 *mot = SBREAK; 00187 motion_phase = 0; 00188 break; 00189 case 1://前進->減速 00190 *mot = (pulse < 2000)? pDuty90:pDuty40; 00191 motion_phase = (pulse < 2000)? 1:2; 00192 break; 00193 case 2://前進->後進 00194 //*mot = (arm_usiro == 1)? pDuty40:nDuty80; 00195 if(arm_usiro == 1) { 00196 *mot = pDuty40; 00197 motion_phase = 2; 00198 } else { 00199 char send_data[1] = {SBREAK}; 00200 motion_phase = 3; 00201 i2c.write(Arm_add, send_data, 1, false); 00202 wait(2); 00203 } 00204 //motion_phase = (arm_usiro == 1)? 2:3; 00205 break; 00206 case 3://後進->減速 00207 *mot = (pulse > 1600)? nDuty90:nDuty40; 00208 motion_phase = (pulse > 1600)? 3:4; 00209 break; 00210 case 4://後進->停止 00211 *mot = (arm_mae == 1)? nDuty40:SBREAK; 00212 motion_phase = (arm_mae == 1)? 4:0; 00213 break; 00214 default: 00215 *mot = SBREAK; 00216 motion_phase = 0; 00217 break; 00218 00219 } 00220 } 00221 00222 void set_drop(char *mot, int num1,int num2) 00223 { 00224 *mot = SBREAK; 00225 if(get_but(num1) != get_but(num2)) { 00226 int butnum = (get_but(num1) > get_but(num2))? num1:num2; 00227 switch (butnum) { 00228 case squ: 00229 *mot = (drop_mae == 1)? pDuty85:SBREAK; 00230 break; 00231 case cro: 00232 *mot = (drop_usiro == 1)? nDuty85:SBREAK; 00233 break; 00234 } 00235 } 00236 } 00237 00238 void set_sheet(char *mot,char mode, int num1, int num2) 00239 { 00240 *mot = SBREAK; 00241 if(get_but(num1) != get_but(num2)) { 00242 int butnum = (get_but(num1) > get_but(num2))? num1:num2; 00243 switch (butnum) { 00244 case up: 00245 *mot = (mode == 'R')? pDuty85:nDuty85; 00246 break; 00247 case dow: 00248 *mot = (mode == 'R')? nDuty85:pDuty85; 00249 break; 00250 } 00251 } 00252 00253 } 00254 00255 void write_data(char address,char data) 00256 { 00257 char send_data[1] = {data}; 00258 i2c.write(address, send_data, 1, false); 00259 }
Generated on Fri Jul 22 2022 00:03:22 by
1.7.2