a
Fork of ARAI45th by
Embed:
(wiki syntax)
Show/hide line numbers
prime.h
00001 00002 /* 00003 primeではPINを制御する関数を扱う。 00004 00005 以下一覧 00006 open,close hand 00007 open close arm 00008 00009 step 00010 move 00011 blueorred 00012 lrsensor 00013 00014 全関数共通で意図しない動作が起きたら基板上のLEDを点灯 00015 */ 00016 PwmOut pwmarm(PC_6); 00017 PwmOut pwmhand(PC_8); 00018 00019 AnalogIn rightsensor(A0); 00020 AnalogIn leftsensor(A1); 00021 00022 AnalogIn armadj(A2); 00023 AnalogIn handadj(A3); 00024 00025 DigitalIn teamSW(PC_11); 00026 DigitalOut teamledblue(PC_10); 00027 DigitalOut teamledred(PC_12); 00028 00029 DigitalOut errorled(LED1); 00030 00031 00032 DigitalIn phase1(PB_8); 00033 DigitalIn phase2(PC_9); 00034 DigitalIn phase4(PB_9); 00035 DigitalIn phase8(PD_2); 00036 00037 PwmOut M1cw(PA_11); 00038 PwmOut M1ccw(PB_15); 00039 PwmOut M2ccw(PB_14); 00040 PwmOut M2cw(PB_13); 00041 00042 DigitalOut PINW(PA_3); 00043 DigitalOut PINX(PA_2); 00044 DigitalOut PINY(PA_10); 00045 DigitalOut PINZ(PB_3); 00046 00047 00048 00049 00050 DigitalOut encordervcc1(PA_6); 00051 DigitalOut encordervcc2(PA_14); 00052 00053 void initencorder(void){ 00054 encordervcc1=1; 00055 encordervcc2=1; 00056 } 00057 00058 00059 00060 void close_hand(void) { 00061 int i,degree; 00062 00063 pwmhand.period_ms(20); //20ms 00064 00065 degree=175; 00066 00067 i=500+degree*1900/180; 00068 pwmhand.pulsewidth_us(i); 00069 00070 00071 } 00072 00073 void close_arm(void) { 00074 PwmOut mypwm(PB_3); 00075 int i,degree; 00076 00077 mypwm.period_ms(20); //20ms 00078 00079 degree=160; 00080 00081 i=500+degree*1900/180; 00082 pwmarm.pulsewidth_us(i); 00083 00084 00085 } 00086 00087 00088 00089 void open_hand(void) { 00090 PwmOut mypwm(PWM_OUT); 00091 int i,degree; 00092 00093 pwmhand.period_ms(20); //20ms 00094 00095 degree=90; 00096 00097 i=500+degree*1900/180; 00098 mypwm.pulsewidth_us(i); 00099 00100 00101 } 00102 00103 00104 00105 void open_arm(void) { 00106 PwmOut mypwm(PWM_OUT); 00107 int i,degree; 00108 00109 mypwm.period_ms(20); //20ms 00110 00111 degree=10; 00112 00113 i=500+degree*1900/180; 00114 pwmarm.pulsewidth_us(i); 00115 00116 00117 } 00118 00119 void step(int degree){ 00120 00121 00122 00123 int puls_times=0; 00124 00125 if(degree>0){ 00126 00127 puls_times=1+(int)(degree/(3.75)); 00128 00129 while(1){ 00130 PINW=1; 00131 PINX=1; 00132 PINY=0; 00133 PINZ=0; 00134 wait_ms(20); 00135 puls_times--; 00136 00137 if(puls_times<0){break;} 00138 00139 PINW=0; 00140 PINX=1; 00141 PINY=1; 00142 PINZ=0; 00143 wait_ms(20); 00144 puls_times--; 00145 00146 if(puls_times<0){break;} 00147 00148 PINW=0; 00149 PINX=0; 00150 PINY=1; 00151 PINZ=1; 00152 wait_ms(20); 00153 puls_times--; 00154 00155 if(puls_times<0){break;} 00156 00157 PINW=1; 00158 PINX=1; 00159 PINY=0; 00160 PINZ=0; 00161 wait_ms(20); 00162 puls_times--; 00163 00164 if(puls_times<0){break;} 00165 } 00166 00167 } 00168 00169 if(degree<0){ 00170 00171 puls_times=(-1)*(1+(int)(degree/(3.75))); 00172 00173 while(1){ 00174 PINW=1; 00175 PINX=1; 00176 PINY=0; 00177 PINZ=0; 00178 wait_ms(20); 00179 puls_times--; 00180 00181 if(puls_times<0){break;} 00182 00183 PINW=1; 00184 PINX=0; 00185 PINY=0; 00186 PINZ=1; 00187 wait_ms(20); 00188 puls_times--; 00189 00190 if(puls_times<0){break;} 00191 00192 PINW=0; 00193 PINX=0; 00194 PINY=1; 00195 PINZ=1; 00196 wait_ms(20); 00197 puls_times--; 00198 00199 if(puls_times<0){break;} 00200 00201 PINW=0; 00202 PINX=1; 00203 PINY=1; 00204 PINZ=0; 00205 wait_ms(20); 00206 puls_times--; 00207 00208 if(puls_times<0){break;} 00209 } 00210 00211 } 00212 00213 } 00214 00215 00216 00217 int sensor(void){ 00218 //センサー読 00219 int right ,left,x,y; 00220 00221 right=rightsensor.read(); 00222 left=leftsensor.read(); 00223 00224 if(right>0.5){x=1;} 00225 else{right=0;} 00226 00227 if(left>0.5){y=1;} 00228 else{left=0;} 00229 00230 return x+2*(y); 00231 //(right,left)=(off,off),(on,off),(off,on),(on,on) 00232 // 0 1 2 3 00233 } 00234 00235 00236 00237 void initmotor(){ 00238 00239 00240 M1cw.period_us(256); 00241 M1ccw.period_us(256); 00242 M2cw.period_us(256); 00243 M2ccw.period_us(256); 00244 00245 } 00246 00247 void move(int right,int left){ 00248 00249 float rightduty,leftduty; 00250 00251 if(right>256){right=256;} 00252 if(left>256){left=256;} 00253 if(right<-256){right=-256;} 00254 if(left<-256){left=-256;} 00255 00256 rightduty=right/256.0; 00257 leftduty=left/256.0; 00258 if(right>0){ 00259 M1cw.write(1-rightduty); 00260 M1ccw.write(1); 00261 }else{ 00262 M1cw.write(1); 00263 M1ccw.write(1+rightduty); 00264 } 00265 00266 if(left>0){ 00267 M2cw.write(1-leftduty); 00268 M2ccw.write(1); 00269 }else{ 00270 M2cw.write(1); 00271 M2ccw.write(1+leftduty); 00272 } 00273 } 00274 00275 00276 int phase(void){ 00277 00278 phase1.mode(PullUp); 00279 phase2.mode(PullUp); 00280 phase4.mode(PullUp); 00281 phase8.mode(PullUp); 00282 00283 int r=0; 00284 00285 r=phase1+2*phase2+4*phase4+8*phase8; 00286 00287 return r; 00288 } 00289 00290 00291 void teamLED(){ 00292 teamSW.mode(PullUp); 00293 if(teamSW){ 00294 teamledblue=1; 00295 teamledred=0; 00296 }else{ 00297 teamledblue=0; 00298 teamledred=1; 00299 00300 } 00301 }
Generated on Fri Jul 15 2022 14:59:36 by
1.7.2
