From a09d92ead4983953006e3d48fb3abf626a168357 Mon Sep 17 00:00:00 2001 From: bap Date: Thu, 21 Mar 2024 20:30:51 +0100 Subject: [PATCH] my commit --- README.md | 8 ++- servo_motor.c | 168 ++++++++++++++++++++++++++++++++++++++++++++++ servo_motor.h | 35 ++++++++++ servo_motor.h.gch | Bin 0 -> 15016 bytes servo_motor.o | Bin 0 -> 12608 bytes 5 files changed, 208 insertions(+), 3 deletions(-) create mode 100644 servo_motor.c create mode 100644 servo_motor.h create mode 100644 servo_motor.h.gch create mode 100644 servo_motor.o diff --git a/README.md b/README.md index b54981f..7a0032e 100644 --- a/README.md +++ b/README.md @@ -1,4 +1,6 @@ # servo_moteurs -# servo_moteurs -# servo_moteurs -# servo_moteurs + +``` +gcc your.c servo_motor.o -o yout_exe (-lpigpio) + +``` diff --git a/servo_motor.c b/servo_motor.c new file mode 100644 index 0000000..f98ed6b --- /dev/null +++ b/servo_motor.c @@ -0,0 +1,168 @@ +#include +#include +#include + +//Define registers address +#define PCA9685_ADDR 0x40 +#define MODE1_REG 0x00 +#define PRE_SCALE 0xFE + +#define LED0_ON_L 0x06 +#define LED0_ON_H 0x07 +#define LED0_OFF_L 0x08 +#define LED0_OFF_H 0x09 + +//Define duty cycle (in a range of 12 bits = 4096) for PWM (min 2%, max 12%) +#define SERVO_MIN 82 // 0.02*4096 +#define SERVO_MAX 492 // 0.12*4096 + +// Fonction pour initialiser le PCA9685 +void initPCA9685(int handle) +{ + // Mettre le PCA9685 en mode de sommeil + i2cWriteByteData(handle, MODE1_REG, 0x10); + + // Configurer la fréquence PWM (par exemple, 50 Hz) + int prescale_val = 25000000 / (4096 * 50) - 1; + i2cWriteByteData(handle, PRE_SCALE, prescale_val); + + // Mettre le PCA9685 en mode de travail normal + i2cWriteByteData(handle, MODE1_REG, 0x00); +} + +// Fonction pour définir la position du servo +void setServoPosition(int handle, int channel, int position) +{ + if (position < 0 || position > 180){ + return; + } + int on_time = SERVO_MIN + (SERVO_MAX - SERVO_MIN) * position / 180 - 1;//temps ou le servo est allumé par rapport à 4096 + int on_time_lsb = on_time & ~(0b1111 << 8);// 8 premiers bits de poids faible du temps ou le servo est allumé + int on_time_msb = on_time >> 8;// 4 premiers bits poids fort de on_time + i2cWriteWordData(handle, LED0_ON_L + 4 * channel, 0); + i2cWriteByteData(handle, LED0_ON_H + 4* channel, 0); + i2cWriteByteData(handle, LED0_OFF_L + 4 * channel, on_time_lsb);//0xCD = 0.05*2^12-1 + if (0 < (on_time >> 8) && (on_time >> 8) < 256){ + i2cWriteByteData(handle, LED0_OFF_H + 4*channel, on_time_msb); + } + else { + i2cWriteByteData(handle, LED0_OFF_H +4*channel, 0); + } +} + +//Change l'angle du servo de maniere precise (jspr par pitié) +void changeServoPositionSynchro(int handle, int channel, int currentPosition, int targetPosition){ + if (targetPosition - currentPosition > 0){ + for (int i = currentPosition+1; i <= targetPosition; i++){ + setServoPosition(handle, channel, i); + gpioSleep(PI_TIME_RELATIVE, 0, 10000); + }//Fo bouger les servo en meme temps aaah donc rajouter les position et channel et info des autres servo + } +} + +//Fonctions pour le bras sur le canal 1 +void baisser_bras(int handle){ + int angle = 107; + for (int i = 1; i <= 107;i++){ + gpioSleep(PI_TIME_RELATIVE, 0, 10000); + setServoPosition(handle, 4, i); + setServoPosition(handle, 5, 107-i); + } +} + +void lever_bras(int handle){ + int angle = 107; + for (int i = 1; i <= 107;i++){ + gpioSleep(PI_TIME_RELATIVE, 0, 10000); + setServoPosition(handle, 4, 107-i); + setServoPosition(handle, 5, i); + } +} + + +//Fonction pour les pinces +void fermer_pince(int handle, int pince){ + int angle; + if (pince < 0 || pince > 2){ + return; + } + switch(pince){ + case 0: + angle = 142; + break; + case 1: + angle = 42; + break; + case 2: + angle = 152; + break; + } + setServoPosition(handle, pince, angle); +//10° de + pour fermer completement +} + +void ouvrir_pince(int handle, int pince){ + int angle; + if (pince < 0 || pince > 2){ + return; + } + switch(pince){ + case 0: + angle = 115; + break; + case 1: + angle = 22; + break; + case 2: + angle = 125; + break; + } + setServoPosition(handle, pince, angle); +} + + + +int main() +{ + int handle; + + if (gpioInitialise() < 0) + { + fprintf(stderr, "Impossible d'initialiser pigpio\n"); + return 1; + } + + handle = i2cOpen(1, PCA9685_ADDR, 0); + + if (handle < 0) + { + fprintf(stderr, "Impossible d'ouvrir la connexion I2C\n"); + gpioTerminate(); + return 1; + } + + initPCA9685(handle); + lever_bras(handle); + gpioSleep(PI_TIME_RELATIVE, 1, 0); + ouvrir_pince(handle, 0); + gpioSleep(PI_TIME_RELATIVE, 1, 0); // Pause de 2 secondes + ouvrir_pince(handle, 1); + gpioSleep(PI_TIME_RELATIVE, 1, 0); // Pause de 2 secondes + ouvrir_pince(handle, 2); + gpioSleep(PI_TIME_RELATIVE, 1, 0); // Pause de 2 secondes + fermer_pince(handle, 0); + gpioSleep(PI_TIME_RELATIVE, 1, 0); // Pause de 2 secondes + fermer_pince(handle, 1); + gpioSleep(PI_TIME_RELATIVE, 1, 0); // Pause de 2 secondes + fermer_pince(handle, 2); + gpioSleep(PI_TIME_RELATIVE, 1, 0); + baisser_bras(handle); + gpioSleep(PI_TIME_RELATIVE, 1, 0); // Pause de 2 secondes + i2cClose(handle); + + // Terminaison de pigpio + gpioTerminate(); + + return 0; +} + diff --git a/servo_motor.h b/servo_motor.h new file mode 100644 index 0000000..adf92a9 --- /dev/null +++ b/servo_motor.h @@ -0,0 +1,35 @@ +#ifndef SERVO_MOTOR +#define SERVO_MOTOR +#include +#include +#include + +//Define registers address +#define PCA9685_ADDR 0x40 +#define MODE1_REG 0x00 +#define PRE_SCALE 0xFE + +#define LED0_ON_L 0x06 +#define LED0_ON_H 0x07 +#define LED0_OFF_L 0x08 +#define LED0_OFF_H 0x09 + +//Define duty cycle (in a range of 12 bits = 4096) for PWM (min 2%, max 12%) +#define SERVO_MIN 82 // 0.02*4096 +#define SERVO_MAX 492 // 0.12*4096 + +void initPCA9685(int handle); + +void setServoPosition(int handle, int channel, int position); + +void changeServoPositionSynchro(int handle, int channel, int currentPosition, int targetPosition); + +void baisser_bras(int handle); + +void lever_bras(int handle); + +void fermer_pince(int handle, int pince); + +void ouvrir_pince(int handle, int pince); + +#endif diff --git a/servo_motor.h.gch b/servo_motor.h.gch new file mode 100644 index 0000000000000000000000000000000000000000..0e53e2228ce9e2736f694b21e1e1e98599644558 GIT binary patch literal 15016 zcmbVTNpl=Wc9!H>ynDRwcAN4-#0)wrYcJa~W}~|r=$HZ;(~AiCk|9$V4r3@vA&C(@ z`RpIy@PF{dKgZ#N!&jet_tD>%FW2fOr(rS(fSpyDFW>v#_m)gN|Ki!xFJF9p|BwIs z**{(W%fJ55zkTDs|LZ^hDV?f9Fv zKCjDo;K%sTYQ}2Ik{6tXPXvDoK-|_YN{j2*gUpzY^ihJ{S@x!m-pVo_uL0_+KbboiN_2lun z%5rt~hZy=TGrV8j?DWvjwmVhp>)9Qi$=>`Je|{gsoJH&9z_F{@R{vUa8Pc7vzX zZ(#5{=7HI6eZ89N*&O4WevL%zrtb>s3G3Rn_VF`ucY0?fOh; zJ;Z+f>Z=#eU)_H?dG?b}zT_m_hSuBF-}mb!wz{6}+Kk()=YEw7(ZHg=S#PebwB1y- z1;njY)pVW8m8$A2tFQyrHCfp<*hqbUUaE_wswvGcnsuOUDtZ3++1cdT{7= zUp8a&tJy|htaezvrBuISKBa*~gJPLq$ERZ1t$E?;YO%ZGE5C;Undm8P8rzcg+;f~S z8-o^k3W~fsn_=uX&A8pK?AAb{m=$i+Q*~L{WNnrejly=@y1D;u(Q>y0 zmzc*+MEH60KRz#-vp>Qk0GkYpp|?+Gw>qDwlbbor^}2!SZnUAl+lXilKrfm;lo`Ws9oA*Zn}ViBwz2X(zVM_2Sjn z+?BYLBc9muS#!oMi+3(pH>+KbrGVe{&G0d|#o6U`s}u?%)CJ-yPqw3)sAwVT*ASR> zVQ4ojwH>+8Y&COZ?nbh)zvi)>Q?1@9KJq zm&rpC3n>x7Vm;#J=dYfJm;?Wy>{O4|RyC>Dm%o6qZ=3O#FJ8TVGf{lOLk%$QZ?~&q zZ6a`gUN4ptjQ@#wVayV{tyXPaD_zjDs8+@rttle;-vC2`Jso_1h0)-;i8>xX{`B+L z|M2qutekdbR#$adwWZ3Tgt8=*5T?7lt)Ur(SdAc^mxD<%yf0J3Ng4v=sWVbz9J3H8 zAnKk5^yKE=^zQc(Ah{ zmX}pQYbTM(i2e9n>o`)KPt+MFp+yZ9z@ekKKf%Xhw}_x`*ASKQFz_>_Owz1v|06x$ z?j{vEhDoH>uV1|YQjmaKsr`N<4sl`Aarr08_8WmDFmv%D>{sw;{b{HD4VE{n*ZXs`pfBK@vzOt5B% zR!n3iyb*T%gzyv*2|TCLA`j5r()KP<#~UOgL6?kd*NCp*RLquVs>$ksqhjckAoD0b zB(vJ|pYr?w?^4?~f<(~Gr+E&%)>(->7G^7uz_w+BFi2Ijss_117pjIPxVr4>`K#C3 zurVi3^h0hK8!RC&6FvczF?j{FnHYEw;>bXL3#`l{?@+}VX)W^3)O8cbHUb34u1gwA z9KZ!2lWykpH8Jbu&3?|E79kdW@#3DQ2_IZvvr<${k-QwE42z1ANN0;HM6FA`Snn^8 z>Eqox@bks$^0MFV^lVsN-dy(pksJ3pm}N4rM_4Xc!tP{bi7Yr4L%fu?eu9$qP9PW| ze;?=6wCS1}VWueC8tGDnQb&Q|nldktMxnw(8ij%q-;3WPWVwcDM0vU6&I~E;|V6SF~dnVD=%;GmI*UT*$y#14{gHYJ-X5vMnOXU+62;UW$oNNHHw2FMXnaQ?CV zC|1JOCQPXQysH*VD$Q~ln=;71LS;kYWK431z@mTcrL z^Ce_)4v6x77zh`6+PVzCp%_N*B-qu$V{BJ$Ra6KgI8qnup`~VW*@Nj#8-~-Dfsuop z&cJm^#e1%|j^v9azAA&4cXxVrvC<#)n=PDqqB7NTR8Rx4(jbA34u$51?yi z^?bR~v+cDIxaS*7k z(sbwa3I#F#Njwv_pCnGzv_qm$mu1sbMTf-Tcuv`Z7dVfH$H>E`aKtW0R*d2im3PI0 z8>Mc84q=Wes>_imf?i72k?c7Nt67*uJ-7#y@XX_=AQ8!;-PIO=J41w6U0gs~qX)HW z0KjkyLn9^!!?@s;w1l7dH6cDria<1{XI@4AhOn04<69WkA>e7IF~}#-+qNCEi`7Ce z(VLx_#DS3a{=^s>MYd7*TkyjNd|rV5B)U|b!$XXIkv>wI2Sf;KDJP3mD!Hj|#1Aa# zatf#rMLVj6#lYFc)SLctg{o+yZ#VsN#k`7)An!85Ms7(e3x;(z2%d2eD`d@+JL^SG z7Y|DxZ<`Z3p?fbIu0xEtS)*PURyU(K4K<9Km^t?^c7_t;BsMRJG%PthOO(j(UsZGRed%R@0`z3u?; zY*>iczGRYiTPT`Pc9mH<9J2Sr5X2-YkObs$Nm9UQGK~GFrtRpt0zmt=sHQ0Rq3@;O zJ+W$VcFY4?2gZS?scNwZo-As-UEGFqh5}8jpgzn%abxwtSM#&Zi{zd#tut4HOC^5v zYmfKK#X#5uaM=oB6gMB^8A3x)>yPB@Q(yh$^DnEIdRndbGg#qw1pf3cL&AoIVK{S-~g-=k;LHC<_;BxsV{i)u{9@z*Zt%XIvi*O z4*jFVq^3V1Cmbg2^4W*yn&^NoraD zg4T*jERl|Bi_xd?5PK+qBFSax4hdTS6em_pnPms2c(cbnF&1M*eIHMIXEZ%RLkA4z zXtSa4Xz>lyQM5Qh$nqj@(Dp*}H$xuSSC^@cT z%(ZT^WutU<(GMYTat4XYBci)l^7Px19G?&!xYWqjCZk#arYBm*wLs|I-~kZPc${z- z1ls9M(vP%@ABue>rdcmY=7VpJ+8BzjNIa7~oG4G;0RqPFaC<%nvI&Gsp$Gx8hsmEH z|8{@DG?T}R#=6FgHsCzun8ZH4&rH#Y-zC|oC4?C;2ElCMkDbyMxU_g zVZ;pO@a<-Hc|Aj`0?8lq)c$UFGegKyAyS#bD6A4nA$F9O=qNFUgqS|QK|{0t08jWt z4}wLtTs`Hv$$@3k4{#g{%mq+6UHNoT)$C!4ZiI@G`c+pC<$c-4=7X?3JG_2i1dhFPheiy%QBcSZ`aV_ zgoGTIhGXl-;3bvYBxy(M{uVclc#o>)nH$dT<>=lWMSCl-ar;)L;W6_!@y{MKGf@`m z7GfqJB=kna$6#Z$@C0YB^#u1ZaKu5BggDbc3?&(iD%niXmFo^V$kmY^Z4bc1zK)7R zr$a$N?zGTp+7m~!1>eF@<{cR)aRF-x4^NKS!ieg5JPJET(;m^0s>_2T? ziXp@bB@;+7o5P(0@iJgT0e1Y5c)6rN(k67$Z^9fQzyIRJGc20(f{!f9Nn`lB0O`KV zO(emiLji0ZP(Un8f?xnNHqT*R;{(x=<}?c^p3Nkx-7U-RC$zks96(+AhM-7Sm2q3Y#3d`vz739~e4uK^48Reor2VNTmE~FEklIPxVixX+ z^I2(fQ^ScBq`t;kUZ}4Tcog3iB4xiSf?QP)?h+AfrfixXq->zwL_QgHyA0=;Eseo| zk{HBg0P+0^u94&##siT9p=Mbu$Z0Yv-TKibQ48~No(WpFV2_I>TEs>SxlOvgTH|gO zP#sR63@`rwKv6`;ul5j(IEV_(1O?_yf>`vi)Z_c~!%gs90E;=6p=AQi8>KmylPp4T zN!Xb*Ar5eawW!h$Cmze~$hZw31e%X9s3t?auz0~d4lP~0Rj`rNGW8U~GJja1=JX>lc%ZNxQfYT_BVm{4(W%APNvN$2yde>RrY$b4N8^?yrx!C?HM=?-=?(oZ5cG&CVsD@! zMO&tPxSj3hI3D~6)jA5tC{7f;5_t%Fb_pK$bT5{JVz3m zV_tQkX7_yJ3nV~Orz8e*vFA$cm&kdQJMJxkxix8K94Xos5$#pL%Pr{tp;1*rvO z!U(6`rz-Mf37ZFnB{(WDAoP)vBv+idxS+|%%{x&L0~8a`L#hxBI-Y4mF}A$hZumGi zBBn7Lai)0y(V_SI9njG7`08PLlF)N zA@H0w;)_aRMM=7&Upk=YL#=90^0S8GXRe+<-WVjlgU`?%CMkr=kS?;{Jqj-N=q^+UMafb*QrbvDbBCN0NHS#|BHYihw}SYD+=@aM_x-@ELVVmnC1I}9e+T5~&ZM4+ t?xr&tln6ley+bHID5fA3Y&^GU@Ic^is{{!*s_;dgO literal 0 HcmV?d00001 diff --git a/servo_motor.o b/servo_motor.o new file mode 100644 index 0000000000000000000000000000000000000000..c3928c43f48a747dc401d22a9b0007e1047c5e71 GIT binary patch literal 12608 zcmcIqeQ;dWb-!=lUU}D&^ev*%NTzl+bi3Eg+F7P#MmUOUG0;!0YDLo+nvkD1hyLP}@*KBd>Vz;<(u{&`g6q?>|iRt}7dBSO$-hVN!@JsQ;h0C_J{FQFiMqJ=& zZntCq@Y;jldv3-~z8$oAsY_hA+yOQ^5*NCKICeBD4wC$X(6I)PF9DvoZF;{mCAu!P zpWL5%iOfIkB9!*5>|cB)SOl^)!pg{u~TffS|}cfMdaG%Ut9rpQJ#vlUR)wB zT!Hv6c7g6MIv^H`mJq~u$rj5m+V*lBPgVQPH)xND7PLWqSKzoSHncw+%lj6Lh5dJb zY#+;kUghH!(Ly$sRNJT*qRr%)v4(*2t8#j!Rb05#3S)*GVtz2DFNIWoMOaX-8o(8I?zUWl@b`=KkMp$UVvt zaqMJ3c5qG@U#Vr_pA(FSl{^=6IMbr?IS1r~RB{+((W+$3#dY%u?X`0Be;waYV2vAG5w~(#VO?CTKgpU z!fVBfOz%e>alLn;t*_5w>zP?>X?vT%o?f52M*H9!Y%BW~)V11>SF3H{25tWzHfzP9 zaU3&_>Dq&@(moBk7#yD{6pPt(&UG?NviYo^P35vh*K;PaqZ8QzEJ;0 zyf4DaNH|1rIIJqG4oTq<83{+IA}jXk&T(i_@|iA_>u}AmnfV}!cfx&<5bUdTd%SJV zHpEC?8$AhPgyeN3H<IFFNjq+<)T-4 zVF&YN{45QJQkOc#YCBG)QimB4w#+nIvVIQhYe*Lb2LtXGBrYu634z zZfjRWH5E501%kBZ3A=G)Pxy}d4fTVq>tRT+t0H14Bm{PCDF)#p4&Nf`>o?cijj$%y zlsP@o!B!;IHHrG32=eB{tSz0|L{Ht4&K7I_oW#PZP74|%y0PvKxuHic=^SkBr-o=d znygn|VIyb-wZBt&6SCx);fCTKwQ5OVzzg4(Mg9E1=ZJT~qe z9QMLsQzauXPb=Y%W=L%$ICWHlcSwz4T?b?dT-?vXmi3FqdBTDM8vKFKP0IqA!4k5% zLrt_5RZv=xL+5xmiYcOBp{Rt(^%Oxlgu>P}u61>$b?P{WAJU ze3O)$zpihB}s;C|B!_F zab&trE*Qnm4RLAr%P{n^dt^OUJLLwfcF0@NRKK+L%0*jbbdzk1OBs_v=X_T2&YvOO}gOSY=sa*J$uT|)2;iz~kpWV?ugH#u_Is%m=lb!o?C zbaCakLT_|#lhHmIab$FrY(6C$K|U;_n@`E+9x40c@tvCj|KdCQ(5=-2cR zc>%xqQTKrB9WU%F6yfAn$R9Y7A0G1xB00D>dDq~sJH_35?sc~Ht#kTT$5zKyIlbe# ztncUCUN@h~rt+Yt(%DV9iM4B--chG_Bvn)f@`X~dx0HV%o6q$6DL5P!$z(Bg+)YAP zQ5-9HzJuqqV)k=x(ig)e&vWy>jzc61BO^uE7lnM%&yKsv@nV`RJm6)0H#y;ZA~}*v zjTVJEN+u_=`C*qB>C#9tohrHl&ZRUaf#=YJt~Z{|r+nayc&_WY#Uv0#ay;c7bv+fG z=cY0g-3X9fk8SD*au5NYZ})8**syl}?IOE+_|q87))T(FJ>{na<*VptT+b7J${Ten zqrnIckXm7@1+2rWi-d?ewb+R=K?#Bj3Y{FMHLoWNDYWv{vV^raoXWY$l2jQLY3@Ato!cFwui>nG!m#Kr{mgSzN@rD2~oBhc^~gGxPHQfnq#fLh$WZQDj?>2^1byOP?h zdi&NaV-apf&3LDRVtsDE2v07ux*EF0&7jli2WmR?3^cBKvhFE);K%<8M-@Ts>rsG0 zr!a00F#-GI-4qKQV!1sT=s&5NYqp>M740u_`#G%H{z$<7J2z-|pzR}Wr$>Wi+NSLR%}7bM62c6F0ZySBvkw@>hZGzlVro|;@OI= z-hos#;e+V>K;HCey$5i;#)BPJGhg%c(*&O5XX7Ie?W-quUe|yXYVc1mAKpt+q3Zyo zFED=I!137~?XRIzW932gDaPM2@NY2g^AggJxaR?%E&lJ;z+bC@|9uVohc)mkHE?5B zyJb?`lDYAD;kw((b$7WN9MCE!imE$B;I3MIh@ls5e4?QnvvKm^bIS4+3}_ z`=pB3E83w~EZlae2Cg2umEWM9M+5vI{%!yd+W#nk2l)}c9x$FDK2rl9;_JlZPX+Mc zOV!61UkCOV1@dr`@%09Nj`3E&gS+mZFuuURpJN>3f%og>G84d^GAhJdjGOVi!?+pG zdyMONma?r67}xP={6oggcs>f?J%M=CJ7P7zY7bY9&3ubOJMaWwE=~Xs&hN?q-c@D` z(I3Er{2c*2$UhXogM9V6N5vWB<2xfP7-x__9l&o=6#9KUfCu?!19*`Cd;kyfUkuwj{WO-+{^s=243KJ78!U4 z2XZQws={0|KL zFBtzL1OE@s=d%WWp8e|mOy@_hkAF1y$JqYQ4E&p{&trSq&YjF(Y2YKwU&pxKpY`*^ z9)tfO^Y1tCH+Wnr!;XGlnKby?&KC^)2-|F0vqHSn#h2hVTF&oLe| z_>Z%W?_02F`!6X~^mDE8zh!;E!128Umb+9{wsVE`od*6M>-!A+L)Py#aP*Jmu&T=T zqm1MK{=lN+)aMm_J~Z=*??AD@m%plWuO6?)_4>SD(aU^&eN7p-zTVFoxZZ#M+Q9XG z_^yHL{rO`9*ZaSS6;F)A>w2htPt{{u^4wf1=DU-=h|w2}Sh`pgF|VK$YR$Om(r7Z9 zA1MHb*&wm;HbD7cDzD{}>LwlQH|_nlh${y6@nZa{B&AAp97^p_pdtuU`bs=#E^Q{r?vZ5+G*) literal 0 HcmV?d00001