File content as of revision 1:8eb45178d28c:

#include "mbed.h"

AnalogIn   F1p(PA_0); 
AnalogIn   F1n(PA_1);
AnalogIn   F2p(PA_4);
AnalogIn   F2n(PB_0);
AnalogIn   F3p(PC_1);
AnalogIn   F3n(PC_0);

DigitalOut myled(PA_5);
PwmOut M1(PA_6);
PwmOut M2(PA_7);
PwmOut M3(PB_6);

Serial UART(SERIAL_TX, SERIAL_RX);

int main() {
    float w1 = 1025, w2 = 620, w3 = 1365;
    int i;
    float fb1 = 0, fb2 = 0, fb3 = 0;
    char c;
    M1.period_ms(20);
    M2.period_ms(20);
    M3.period_ms(20);
    // Home position
    M1.pulsewidth_us(1900);
    M2.pulsewidth_us(600);
    M3.pulsewidth_us(2000);
    wait(2.0);
    
     // first move
    for(i=0;i<500;i++)
    {
        M1.pulsewidth_us(1900 - (unsigned int)(i*1.75));
        printf("%d",1900 - (unsigned int)(i*0.853));
    }
    wait(2.0);
    i=0;
    for(i=0;i<1000;i++)
    {
        M3.pulsewidth_us(2000 - (unsigned int)(i*0.653));
        printf("%d",2000 - (unsigned int)(i*0.653));
    }
    
    M1.pulsewidth_us(1025);
    M2.pulsewidth_us(591);
    M3.pulsewidth_us(1347);
    wait(2.0);
    
    M1.pulsewidth_us(1005);
    M2.pulsewidth_us(626);
    M3.pulsewidth_us(1371);
    wait(2.0);
    
    M1.pulsewidth_us(991);
    M2.pulsewidth_us(630);
    M3.pulsewidth_us(1417);
    wait(2.0);
    
    M1.pulsewidth_us(985);
    M2.pulsewidth_us(636);
    M3.pulsewidth_us(1430);
    wait(2.0);
    
    M1.pulsewidth_us(963);
    M2.pulsewidth_us(642);
    M3.pulsewidth_us(1445);
    wait(2.0);
    
    M1.pulsewidth_us(953);
    M2.pulsewidth_us(680);
    M3.pulsewidth_us(1451);
    wait(2.0);
    
    M1.pulsewidth_us(947);
    M2.pulsewidth_us(688);
    M3.pulsewidth_us(1461);
    wait(2.0);
    
    M1.pulsewidth_us(935);
    M2.pulsewidth_us(710);
    M3.pulsewidth_us(1467);
    wait(2.0);
    
    M1.pulsewidth_us(913);
    M2.pulsewidth_us(728);
    M3.pulsewidth_us(1485);
    wait(2.0);
    
    M1.pulsewidth_us(909);
    M2.pulsewidth_us(750);
    M3.pulsewidth_us(1503);
    wait(2.0);
    
    M1.pulsewidth_us(913);
    M2.pulsewidth_us(774);
    M3.pulsewidth_us(1519);
    wait(2.0);
    
    M1.pulsewidth_us(977);
    M2.pulsewidth_us(792);
    M3.pulsewidth_us(1545);
    wait(2.0);
    
    M1.pulsewidth_us(999);
    M2.pulsewidth_us(828);
    M3.pulsewidth_us(1545);
    wait(2.0);
    
    M1.pulsewidth_us(999);
    M2.pulsewidth_us(855);
    M3.pulsewidth_us(1570);
    wait(2.0);
    
    M1.pulsewidth_us(1023);
    M2.pulsewidth_us(863);
    M3.pulsewidth_us(1592);
    wait(2.0);
    
    M1.pulsewidth_us(1045);
    M2.pulsewidth_us(875);
    M3.pulsewidth_us(1590);
    wait(2.0);
    
    M1.pulsewidth_us(1071);
    M2.pulsewidth_us(890);
    M3.pulsewidth_us(1605);
    wait(2.0);
    
    M1.pulsewidth_us(1085);
    M2.pulsewidth_us(898);
    M3.pulsewidth_us(1590);
    wait(2.0);
    
    M1.pulsewidth_us(1109);
    M2.pulsewidth_us(910);
    M3.pulsewidth_us(1590);
    wait(2.0);
    
    M1.pulsewidth_us(1151);
    M2.pulsewidth_us(930);
    M3.pulsewidth_us(1611);
    wait(2.0);
    
    M1.pulsewidth_us(1163);
    M2.pulsewidth_us(906);
    M3.pulsewidth_us(1575);
    wait(2.0);
    
    M1.pulsewidth_us(1169);
    M2.pulsewidth_us(864);
    M3.pulsewidth_us(1533);
    wait(2.0);
    
    M1.pulsewidth_us(1173);
    M2.pulsewidth_us(794);
    M3.pulsewidth_us(1519);
    wait(2.0);
    
    M1.pulsewidth_us(1189);
    M2.pulsewidth_us(790);
    M3.pulsewidth_us(1511);
    wait(2.0);
    
    M1.pulsewidth_us(1215);
    M2.pulsewidth_us(776);
    M3.pulsewidth_us(1479);
    wait(2.0);
    
    M1.pulsewidth_us(1219);
    M2.pulsewidth_us(752);
    M3.pulsewidth_us(1455);
    wait(2.0);
    
    M1.pulsewidth_us(1223);
    M2.pulsewidth_us(732);
    M3.pulsewidth_us(1429);
    wait(2.0);
    
    M1.pulsewidth_us(1225);
    M2.pulsewidth_us(704);
    M3.pulsewidth_us(1405);
    wait(2.0);
    
    M1.pulsewidth_us(1210);
    M2.pulsewidth_us(676);
    M3.pulsewidth_us(1385);
    wait(2.0);
    
    M1.pulsewidth_us(1180);
    M2.pulsewidth_us(650);
    M3.pulsewidth_us(1370);
    wait(2.0);
    
    M1.pulsewidth_us(1156);
    M2.pulsewidth_us(614);
    M3.pulsewidth_us(1344);
    wait(2.0);
    
    M1.pulsewidth_us(1122);
    M2.pulsewidth_us(596);
    M3.pulsewidth_us(1334);
    wait(2.0);
    
    M1.pulsewidth_us(1104);
    M2.pulsewidth_us(586);
    M3.pulsewidth_us(1330);
    wait(2.0);
    
    M1.pulsewidth_us(1090);
    M2.pulsewidth_us(566);
    M3.pulsewidth_us(1328);
    wait(2.0);
    
    M1.pulsewidth_us(1064);
    M2.pulsewidth_us(560);
    M3.pulsewidth_us(1324);
    wait(2.0);
    
    M1.pulsewidth_us(1048);
    M2.pulsewidth_us(558);
    M3.pulsewidth_us(1320);
    wait(2.0);
    
    M1.pulsewidth_us(1016);
    M2.pulsewidth_us(580);
    M3.pulsewidth_us(1330);
    wait(2.0);
    
    w1 = 1016;
    w2 = 580;
    w3 = 1330;
    
    //=============================================
    
   
    
   


    
    
    UART.baud(9600); // Set BuadRate
    printf("PW1 = %f us,PW2 = %f us,PW3 = %f us \n\r",w1,w2,w3);
    while(1) {
        c = UART.getc();
        if(c == '1')
        {
            w1 = w1 + 2;
        }
        else if(c == '2')
        {
            w1 = w1 - 2;
        }
        else if(c == '3')
        {
            w2 = w2 + 2;
        }
        else if(c == '4')
        {
            w2 = w2 - 2;
        }
        else if(c == '5')
        {
            w3 = w3 + 2;
        }
        else if(c == '6')
        {
            w3 = w3 - 2;
        }
        
        printf("PW1 = %f us,PW2 = %f us,PW3 = %f us \n\r",w1,w2,w3);
        M1.pulsewidth_us(w1);
        M2.pulsewidth_us(w2);
        M3.pulsewidth_us(w3);
        
        for(i=0;i<=9;i++)
        {
            fb1 = fb1 + ((F1p-F1n)*3.3f);
            fb2 = fb2 + ((F2p-F2n)*3.3f);
            fb3 = fb3 + ((F3p-F3n)*3.3f);
        }
        printf("FB1 = %f V, FB2 = %f V, FB3 = %f V\r\r\n",(fb1/10),(fb2/10),(fb3/10));
        fb1 = 0;
        fb2 = 0;
        fb3 = 0;
        myled = !myled;
        }
}