Thomas Giraud-Sauveur / Mbed 2 deprecated Gimbal_ENSEA

pour simone

Dependencies:   MPU6050 mbed

mycontroller.cpp@2:146ff0747375, 2016-06-03 (annotated)

Committer:
ThomasGS
Date:
Fri Jun 03 14:50:37 2016 +0000
Revision:
2:146ff0747375
Parent:
0:63c6db89607f 
version avec moteur parametre

Who changed what in which revision?

UserRevisionLine numberNew 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 2:146ff0747375 8 //PwmOut pwm[] = {(PA_10),(PA_6),(PB_6)};
ThomasGS 2:146ff0747375 9 //DigitalOut enableA(PA_7);
ThomasGS 2:146ff0747375 10 //DigitalOut enableB(PA_9);
ThomasGS 0:63c6db89607f 11
ThomasGS 2:146ff0747375 12 PwmOut pwm[] = {(PB_1),(PB_10),(PB_5)};
ThomasGS 2:146ff0747375 13 DigitalOut enableA(PB_4);
ThomasGS 2:146ff0747375 14 DigitalOut enableB(PC_4);
ThomasGS 0:63c6db89607f 15
ThomasGS 0:63c6db89607f 16
ThomasGS 2:146ff0747375 17 brushlessservo::brushlessservo(PwmOut &p_pwm1,PwmOut &p_pwm2,PwmOut &p_pwm3,DigitalOut &p_enableA,DigitalOut &p_enableB) : pwm1(p_pwm1), pwm2(p_pwm2), pwm3(p_pwm3), enableA(p_enableA), enableB(p_enableB)
ThomasGS 0:63c6db89607f 18 {
ThomasGS 2:146ff0747375 19 //brushlessservo::brushlessservo()
ThomasGS 2:146ff0747375 20 //{
ThomasGS 2:146ff0747375 21
ThomasGS 0:63c6db89607f 22 sineArraySize = sizeof(pwmSin)/sizeof(int);
ThomasGS 0:63c6db89607f 23 int phaseShift = sineArraySize / 3;
ThomasGS 0:63c6db89607f 24
ThomasGS 0:63c6db89607f 25 enableA=1;
ThomasGS 0:63c6db89607f 26 enableB=1;
ThomasGS 0:63c6db89607f 27
ThomasGS 0:63c6db89607f 28
ThomasGS 0:63c6db89607f 29 phase[0]=0;
ThomasGS 0:63c6db89607f 30 phase[1]=phase[0]+phaseShift;
ThomasGS 0:63c6db89607f 31 phase[2]=phase[1]+phaseShift;
ThomasGS 0:63c6db89607f 32
ThomasGS 0:63c6db89607f 33 direction=1;
ThomasGS 2:146ff0747375 34 /*
ThomasGS 0:63c6db89607f 35 for(int i = 0;i<3;i++){
ThomasGS 2:146ff0747375 36 pwm[i]=pwmSin[phase[i]]/255.0;
ThomasGS 0:63c6db89607f 37 pwm[i].period(1/20000.0);
ThomasGS 0:63c6db89607f 38 };
ThomasGS 2:146ff0747375 39 */
ThomasGS 2:146ff0747375 40 pwm1.period(1/20000.0);
ThomasGS 2:146ff0747375 41 pwm2.period(1/20000.0);
ThomasGS 2:146ff0747375 42 pwm3.period(1/20000.0);
ThomasGS 2:146ff0747375 43
ThomasGS 2:146ff0747375 44 pwm1=pwmSin[phase[0]]/255.0;
ThomasGS 2:146ff0747375 45 pwm2=pwmSin[phase[1]]/255.0;
ThomasGS 2:146ff0747375 46 pwm3=pwmSin[phase[2]]/255.0;
ThomasGS 2:146ff0747375 47
ThomasGS 0:63c6db89607f 48 realtheta=0;
ThomasGS 0:63c6db89607f 49
ThomasGS 0:63c6db89607f 50 }
ThomasGS 0:63c6db89607f 51
ThomasGS 0:63c6db89607f 52 void brushlessservo::gotothetha(int32_t theta)
ThomasGS 0:63c6db89607f 53 {
ThomasGS 0:63c6db89607f 54 int32_t theta_diff;
ThomasGS 0:63c6db89607f 55 theta_diff = theta - realtheta;
ThomasGS 0:63c6db89607f 56
ThomasGS 0:63c6db89607f 57 if(theta_diff<0)
ThomasGS 0:63c6db89607f 58 {
ThomasGS 0:63c6db89607f 59 direction=0;
ThomasGS 0:63c6db89607f 60 theta_diff=-theta_diff;
ThomasGS 0:63c6db89607f 61 }
ThomasGS 0:63c6db89607f 62 else
ThomasGS 0:63c6db89607f 63 {
ThomasGS 0:63c6db89607f 64 direction=1;
ThomasGS 0:63c6db89607f 65 }
ThomasGS 0:63c6db89607f 66
ThomasGS 0:63c6db89607f 67
ThomasGS 0:63c6db89607f 68 for(int i =0;i<theta_diff/5;i++)
ThomasGS 0:63c6db89607f 69 {
ThomasGS 2:146ff0747375 70 /* for(int j=0;j<3;j++)
ThomasGS 0:63c6db89607f 71 {
ThomasGS 0:63c6db89607f 72 if(direction==1){
ThomasGS 0:63c6db89607f 73 phase[j]+=1;
ThomasGS 0:63c6db89607f 74 phase[j]%=sineArraySize;
ThomasGS 0:63c6db89607f 75 }
ThomasGS 0:63c6db89607f 76 else
ThomasGS 0:63c6db89607f 77 {
ThomasGS 0:63c6db89607f 78 phase[j]-=1;
ThomasGS 0:63c6db89607f 79 phase[j]%=sineArraySize;
ThomasGS 0:63c6db89607f 80 if(phase[j]<0)
ThomasGS 0:63c6db89607f 81 {
ThomasGS 0:63c6db89607f 82 phase[j]+=sineArraySize;
ThomasGS 0:63c6db89607f 83 }
ThomasGS 0:63c6db89607f 84 }
ThomasGS 0:63c6db89607f 85 pwm[j]=pwmSin[phase[j]]/255.0;
ThomasGS 0:63c6db89607f 86 }
ThomasGS 2:146ff0747375 87 */
ThomasGS 2:146ff0747375 88 if(direction==1){
ThomasGS 2:146ff0747375 89 phase[0]+=1;
ThomasGS 2:146ff0747375 90 phase[1]+=1;
ThomasGS 2:146ff0747375 91 phase[2]+=1;
ThomasGS 2:146ff0747375 92
ThomasGS 2:146ff0747375 93 phase[0]%=sineArraySize;
ThomasGS 2:146ff0747375 94 phase[1]%=sineArraySize;
ThomasGS 2:146ff0747375 95 phase[2]%=sineArraySize;
ThomasGS 2:146ff0747375 96 }
ThomasGS 2:146ff0747375 97 else
ThomasGS 2:146ff0747375 98 {
ThomasGS 2:146ff0747375 99 phase[0]-=1;
ThomasGS 2:146ff0747375 100 phase[1]-=1;
ThomasGS 2:146ff0747375 101 phase[2]-=1;
ThomasGS 2:146ff0747375 102
ThomasGS 2:146ff0747375 103 phase[0]%=sineArraySize;
ThomasGS 2:146ff0747375 104 phase[1]%=sineArraySize;
ThomasGS 2:146ff0747375 105 phase[2]%=sineArraySize;
ThomasGS 2:146ff0747375 106 if(phase[0]<0){
ThomasGS 2:146ff0747375 107 phase[0]+=sineArraySize;
ThomasGS 2:146ff0747375 108 }
ThomasGS 2:146ff0747375 109 if(phase[1]<0){
ThomasGS 2:146ff0747375 110 phase[1]+=sineArraySize;
ThomasGS 2:146ff0747375 111 }
ThomasGS 2:146ff0747375 112 if(phase[2]<0){
ThomasGS 2:146ff0747375 113 phase[2]+=sineArraySize;
ThomasGS 2:146ff0747375 114 }
ThomasGS 2:146ff0747375 115 }
ThomasGS 2:146ff0747375 116 pwm1=pwmSin[phase[0]]/255.0;
ThomasGS 2:146ff0747375 117 pwm2=pwmSin[phase[1]]/255.0;
ThomasGS 2:146ff0747375 118 pwm3=pwmSin[phase[2]]/255.0;
ThomasGS 2:146ff0747375 119
ThomasGS 2:146ff0747375 120
ThomasGS 0:63c6db89607f 121 direction?realtheta+=5:realtheta-=5;
ThomasGS 0:63c6db89607f 122 wait_ms(1);
ThomasGS 0:63c6db89607f 123 }
ThomasGS 0:63c6db89607f 124 }
ThomasGS 0:63c6db89607f 125
ThomasGS 0:63c6db89607f 126
ThomasGS 0:63c6db89607f 127 void brushlessservo::test()
ThomasGS 0:63c6db89607f 128 {
ThomasGS 2:146ff0747375 129 /*
ThomasGS 0:63c6db89607f 130 for(int j=0;j<3;j++){
ThomasGS 0:63c6db89607f 131 pwm[j]=0.5;
ThomasGS 0:63c6db89607f 132 }
ThomasGS 2:146ff0747375 133 */
ThomasGS 0:63c6db89607f 134 }
ThomasGS 0:63c6db89607f 135
ThomasGS 0:63c6db89607f 136
ThomasGS 0:63c6db89607f 137
ThomasGS 0:63c6db89607f 138 void brushlessservo::updatepwm()
ThomasGS 0:63c6db89607f 139 {
ThomasGS 0:63c6db89607f 140
ThomasGS 0:63c6db89607f 141 }