涼太郎 中村
/
ARAI45th
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Fork of
locate_by_hello_enc
by 
Tk A
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 Sun Jul 17 2022 23:05:29 by
1.7.2