File content as of revision 1:b3ee94985eca:

#include "pwm.h"

MotorDriver::MotorDriver() : M1(PA_6), M2(PA_7), M3(PB_6) {
    unsigned int i;
    M1.period_ms(20);
    M2.period_ms(20);
    M3.period_ms(20);
    
    M1.pulsewidth_us(1900);
    M2.pulsewidth_us(600);
    M3.pulsewidth_us(2000);
    wait(2.0f);
    // first move
    for(i=0;i<1000;i++)
    {
        M1.pulsewidth_us(1900 - (unsigned int)(i*0.853));
        printf("%d",1900 - (unsigned int)(i*0.853));
    }
    for(i=0;i<1000;i++)
    {
        M3.pulsewidth_us(2000 - (unsigned int)(i*0.626));
    }
    M2.pulsewidth_us(591);
    wait(2.0f);
    
    // Look up table
    
    du1[0] = 1025;
    du2[0] = 591;
    du3[0] = 1347;
    
    du1[1] = 1005;
    du2[1] = 626;
    du3[1] = 1371;
    
    du1[2] = 991;
    du2[2] = 630;
    du3[2] = 1417;
    
    du1[3] = 985;
    du2[3] = 636;
    du3[3] = 1430;
    
    du1[4] = 963;
    du2[4] = 642;
    du3[4] = 1445;
    
    du1[5] = 953;
    du2[5] = 680;
    du3[5] = 1461;
    
    du1[6] = 947;
    du2[6] = 688;
    du3[6] = 1461;
    
    du1[7] = 935;
    du2[7] = 710;
    du3[7] = 1467;
    
    du1[8] = 913;
    du2[8] = 728;
    du3[8] = 1485;
    
    du1[9] = 909;
    du2[9] = 750;
    du3[9] = 1503;
    
    du1[10] = 913;
    du2[10] = 774;
    du3[10] = 1519;
    
    du1[11] = 977;
    du2[11] = 792;
    du3[11] = 1570;
    
    du1[12] = 999;
    du2[12] = 828;
    du3[12] = 1545;
    
    du1[13] = 999;
    du2[13] = 855;
    du3[13] = 1570;
    
    du1[14] = 1023;
    du2[14] = 863;
    du3[14] = 1592;
    
    du1[15] = 1045;
    du2[15] = 875;
    du3[15] = 1590;
    
    du1[16] = 1071;
    du2[16] = 890;
    du3[16] = 1605;
    
    du1[17] = 1085;
    du2[17] = 898;
    du3[17] = 1590;
    
    du1[18] = 1109;
    du2[18] = 910;
    du3[18] = 1590;
    
    du1[19] = 1151;
    du2[19] = 930;
    du3[19] = 1611;
    
    du1[20] = 1163;
    du2[20] = 906;
    du3[20] = 1575;
    
    du1[21] = 1169;
    du2[21] = 864;
    du3[21] = 1533;
    
    du1[22] = 1173;
    du2[22] = 794;
    du3[22] = 1519;
    
    du1[23] = 1189;
    du2[23] = 790;
    du3[23] = 1511;
    
    du1[24] = 1215;
    du2[24] = 776;
    du3[24] = 1479;
    
    du1[25] = 1219;
    du2[25] = 752;
    du3[25] = 1455;
    
    du1[26] = 1223;
    du2[26] = 732;
    du3[26] = 1429;
    
    du1[27] = 1225;
    du2[27] = 704;
    du3[27] = 1405;
    
    du1[28] = 1210;
    du2[28] = 676;
    du3[28] = 1385;
    
    du1[29] = 1180;
    du2[29] = 650;
    du3[29] = 1370;
    
    du1[30] = 1156;
    du2[30] = 614;
    du3[30] = 1344;
    
    du1[31] = 1122;
    du2[31] = 596;
    du3[31] = 1334;
    
    du1[32] = 1104;
    du2[32] = 586;
    du3[32] = 1330;
    
    du1[33] = 1090;
    du2[33] = 566;
    du3[33] = 1328;
    
    du1[34] = 1064;
    du2[34] = 560;
    du3[34] = 1324;
    
    du1[35] = 1048;
    du2[35] = 558;
    du3[35] = 1320;
    
    du1[36] = 1016;
    du2[36] = 580;
    du3[36] = 1330;

}

void MotorDriver::Actuate() {
    M1.pulsewidth_us(D1);
    M2.pulsewidth_us(D2);
    M3.pulsewidth_us(D3);
    //printf("d1 = %d, d2 = %d, d3 = %d\r\r",D1,D2,D3);
}

// 100/9
void MotorDriver::Angle2Duty(float a, float b, float c){
    float k;
    
    k =  (float) (100.0/9.0); // degree to radian
    D1 = (unsigned int) (k*a);
    D2 = (unsigned int) (k*b);
    D3 = (unsigned int) (k*c);
}

void MotorDriver::DirectDrive(unsigned int d1, unsigned int d2, unsigned int d3){
    M1.pulsewidth_us(d1);
    M2.pulsewidth_us(d2);
    M3.pulsewidth_us(d3);
    printf("PW1 = %d us,PW2 = %d us,PW3 = %d us \n\r",d1,d2,d3);

}
void MotorDriver::DriveTable(unsigned int index){
    M1.pulsewidth_us(du1[index]);
    M2.pulsewidth_us(du2[index]);
    M3.pulsewidth_us(du3[index]);
}


unsigned int MotorDriver::GetD1 (void){
    return D1;
}
unsigned int MotorDriver::GetD2 (void){
    return D2;
}
unsigned int MotorDriver::GetD3 (void){
    return D3;
}
unsigned int MotorDriver::Getdu1 (unsigned int i){
    return du1[i];
}
unsigned int MotorDriver::Getdu2 (unsigned int i){
    return du2[i];
}
unsigned int MotorDriver::Getdu3 (unsigned int i){
    return du3[i];
}