wesh gros, ajout des ref dans brushlessservo
Fork of Gimbal_ENSEA by
mycontroller.cpp@2:d0606d66af96, 2016-06-03 (annotated)
- Committer:
- sype
- Date:
- Fri Jun 03 14:17:35 2016 +0000
- Revision:
- 2:d0606d66af96
- Parent:
- 0:63c6db89607f
Ajout ref brushlessservo
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
ThomasGS | 0:63c6db89607f | 1 | #include "mycontroller.h" |
ThomasGS | 0:63c6db89607f | 2 | |
ThomasGS | 0:63c6db89607f | 3 | //const int pwmSin[] = {128, 132, 136, 140, 143, 147, 151, 155, 159, 162, 166, 170, 174, 178, 181, 185, 189, 192, 196, 200, 203, 207, 211, 214, 218, 221, 225, 228, 232, 235, 238, 239, 240, 241, 242, 243, 244, 245, 246, 247, 248, 248, 249, 250, 250, 251, 252, 252, 253, 253, 253, 254, 254, 254, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 254, 254, 254, 253, 253, 253, 252, 252, 251, 250, 250, 249, 248, 248, 247, 246, 245, 244, 243, 242, 241, 240, 239, 238, 239, 240, 241, 242, 243, 244, 245, 246, 247, 248, 248, 249, 250, 250, 251, 252, 252, 253, 253, 253, 254, 254, 254, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 254, 254, 254, 253, 253, 253, 252, 252, 251, 250, 250, 249, 248, 248, 247, 246, 245, 244, 243, 242, 241, 240, 239, 238, 235, 232, 228, 225, 221, 218, 214, 211, 207, 203, 200, 196, 192, 189, 185, 181, 178, 174, 170, 166, 162, 159, 155, 151, 147, 143, 140, 136, 132, 128, 124, 120, 116, 113, 109, 105, 101, 97, 94, 90, 86, 82, 78, 75, 71, 67, 64, 60, 56, 53, 49, 45, 42, 38, 35, 31, 28, 24, 21, 18, 17, 16, 15, 14, 13, 12, 11, 10, 9, 8, 8, 7, 6, 6, 5, 4, 4, 3, 3, 3, 2, 2, 2, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 2, 2, 2, 3, 3, 3, 4, 4, 5, 6, 6, 7, 8, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 17, 16, 15, 14, 13, 12, 11, 10, 9, 8, 8, 7, 6, 6, 5, 4, 4, 3, 3, 3, 2, 2, 2, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 2, 2, 2, 3, 3, 3, 4, 4, 5, 6, 6, 7, 8, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 21, 24, 28, 31, 35, 38, 42, 45, 49, 53, 56, 60, 64, 67, 71, 75, 78, 82, 86, 90, 94, 97, 101, 105, 109, 113, 116, 120, 124}; |
ThomasGS | 0:63c6db89607f | 4 | |
ThomasGS | 0:63c6db89607f | 5 | const int pwmSin[102] = {127,135,143,150,158,166,173,180,187,194,201,207,213,219,224,229,233,237,241,244,247,250,252,253,254,254,254,254,253,252,250,247,244,241,237,233,229,224,219,213,207,201,194,187,180,173,166,158,150,143,135,127,119,111,104,96,88,81,74,67,60,53,47,41,35,30,25,21,17,13,10,7,4,2,1,0,0,0,0,1,2,4,7,10,13,17,21,25,30,35,41,47,53,60,67,74,81,88,96,104,111,119}; |
ThomasGS | 0:63c6db89607f | 6 | |
ThomasGS | 0:63c6db89607f | 7 | |
ThomasGS | 0:63c6db89607f | 8 | PwmOut pwm[] = {(PA_10),(PA_6),(PB_6)}; |
ThomasGS | 0:63c6db89607f | 9 | DigitalOut enableA(PA_7); |
ThomasGS | 0:63c6db89607f | 10 | DigitalOut enableB(PA_9); |
ThomasGS | 0:63c6db89607f | 11 | |
ThomasGS | 0:63c6db89607f | 12 | |
ThomasGS | 0:63c6db89607f | 13 | |
sype | 2:d0606d66af96 | 14 | brushlessservo::brushlessservo(PwmOut &p_pwm1,PwmOut &p_pwm2,PwmOut &p_pwm3) : pwm1(p_pwm1), pwm2(p_pwm2), pwm3(p_pwm3) |
ThomasGS | 0:63c6db89607f | 15 | { |
ThomasGS | 0:63c6db89607f | 16 | /* |
ThomasGS | 0:63c6db89607f | 17 | pwm[1]=phaseA; |
ThomasGS | 0:63c6db89607f | 18 | pwm[2]=phaseB; |
ThomasGS | 0:63c6db89607f | 19 | pwm[3]=phaseC; |
ThomasGS | 0:63c6db89607f | 20 | enableA=EENNAABLE1; |
ThomasGS | 0:63c6db89607f | 21 | enableB=EENNAABLE2; |
ThomasGS | 0:63c6db89607f | 22 | */ |
ThomasGS | 0:63c6db89607f | 23 | |
ThomasGS | 0:63c6db89607f | 24 | sineArraySize = sizeof(pwmSin)/sizeof(int); |
ThomasGS | 0:63c6db89607f | 25 | int phaseShift = sineArraySize / 3; |
ThomasGS | 0:63c6db89607f | 26 | |
ThomasGS | 0:63c6db89607f | 27 | enableA=1; |
ThomasGS | 0:63c6db89607f | 28 | enableB=1; |
ThomasGS | 0:63c6db89607f | 29 | |
ThomasGS | 0:63c6db89607f | 30 | |
ThomasGS | 0:63c6db89607f | 31 | phase[0]=0; |
ThomasGS | 0:63c6db89607f | 32 | phase[1]=phase[0]+phaseShift; |
ThomasGS | 0:63c6db89607f | 33 | phase[2]=phase[1]+phaseShift; |
ThomasGS | 0:63c6db89607f | 34 | |
ThomasGS | 0:63c6db89607f | 35 | direction=1; |
ThomasGS | 0:63c6db89607f | 36 | |
ThomasGS | 0:63c6db89607f | 37 | for(int i = 0;i<3;i++){ |
ThomasGS | 0:63c6db89607f | 38 | pwm[i]=phase[i]; |
ThomasGS | 0:63c6db89607f | 39 | pwm[i].period(1/20000.0); |
ThomasGS | 0:63c6db89607f | 40 | }; |
ThomasGS | 0:63c6db89607f | 41 | |
ThomasGS | 0:63c6db89607f | 42 | realtheta=0; |
ThomasGS | 0:63c6db89607f | 43 | |
ThomasGS | 0:63c6db89607f | 44 | } |
ThomasGS | 0:63c6db89607f | 45 | |
ThomasGS | 0:63c6db89607f | 46 | void brushlessservo::gotothetha(int32_t theta) |
ThomasGS | 0:63c6db89607f | 47 | { |
ThomasGS | 0:63c6db89607f | 48 | int32_t theta_diff; |
ThomasGS | 0:63c6db89607f | 49 | theta_diff = theta - realtheta; |
ThomasGS | 0:63c6db89607f | 50 | |
ThomasGS | 0:63c6db89607f | 51 | if(theta_diff<0) |
ThomasGS | 0:63c6db89607f | 52 | { |
ThomasGS | 0:63c6db89607f | 53 | direction=0; |
ThomasGS | 0:63c6db89607f | 54 | theta_diff=-theta_diff; |
ThomasGS | 0:63c6db89607f | 55 | } |
ThomasGS | 0:63c6db89607f | 56 | else |
ThomasGS | 0:63c6db89607f | 57 | { |
ThomasGS | 0:63c6db89607f | 58 | direction=1; |
ThomasGS | 0:63c6db89607f | 59 | } |
ThomasGS | 0:63c6db89607f | 60 | |
ThomasGS | 0:63c6db89607f | 61 | |
ThomasGS | 0:63c6db89607f | 62 | for(int i =0;i<theta_diff/5;i++) |
ThomasGS | 0:63c6db89607f | 63 | { |
ThomasGS | 0:63c6db89607f | 64 | for(int j=0;j<3;j++) |
ThomasGS | 0:63c6db89607f | 65 | { |
ThomasGS | 0:63c6db89607f | 66 | if(direction==1){ |
ThomasGS | 0:63c6db89607f | 67 | phase[j]+=1; |
ThomasGS | 0:63c6db89607f | 68 | phase[j]%=sineArraySize; |
ThomasGS | 0:63c6db89607f | 69 | } |
ThomasGS | 0:63c6db89607f | 70 | else |
ThomasGS | 0:63c6db89607f | 71 | { |
ThomasGS | 0:63c6db89607f | 72 | phase[j]-=1; |
ThomasGS | 0:63c6db89607f | 73 | phase[j]%=sineArraySize; |
ThomasGS | 0:63c6db89607f | 74 | if(phase[j]<0) |
ThomasGS | 0:63c6db89607f | 75 | { |
ThomasGS | 0:63c6db89607f | 76 | phase[j]+=sineArraySize; |
ThomasGS | 0:63c6db89607f | 77 | } |
ThomasGS | 0:63c6db89607f | 78 | } |
ThomasGS | 0:63c6db89607f | 79 | pwm[j]=pwmSin[phase[j]]/255.0; |
ThomasGS | 0:63c6db89607f | 80 | } |
ThomasGS | 0:63c6db89607f | 81 | direction?realtheta+=5:realtheta-=5; |
ThomasGS | 0:63c6db89607f | 82 | wait_ms(1); |
ThomasGS | 0:63c6db89607f | 83 | } |
ThomasGS | 0:63c6db89607f | 84 | } |
ThomasGS | 0:63c6db89607f | 85 | |
ThomasGS | 0:63c6db89607f | 86 | |
ThomasGS | 0:63c6db89607f | 87 | void brushlessservo::test() |
ThomasGS | 0:63c6db89607f | 88 | { |
ThomasGS | 0:63c6db89607f | 89 | for(int j=0;j<3;j++){ |
ThomasGS | 0:63c6db89607f | 90 | pwm[j]=0.5; |
ThomasGS | 0:63c6db89607f | 91 | } |
ThomasGS | 0:63c6db89607f | 92 | |
ThomasGS | 0:63c6db89607f | 93 | } |
ThomasGS | 0:63c6db89607f | 94 | |
ThomasGS | 0:63c6db89607f | 95 | |
ThomasGS | 0:63c6db89607f | 96 | |
ThomasGS | 0:63c6db89607f | 97 | void brushlessservo::updatepwm() |
ThomasGS | 0:63c6db89607f | 98 | { |
ThomasGS | 0:63c6db89607f | 99 | |
ThomasGS | 0:63c6db89607f | 100 | } |