File content as of revision 5:1c95260515c1:

#include "mbed.h"

PwmOut pwmarm(PC_6);
PwmOut pwmhand(PC_8);
DigitalOut led(LED1);
PwmOut M1cw(PA_11);
PwmOut M1ccw(PB_15);
PwmOut M2ccw(PB_14);
PwmOut M2cw(PB_13);
float PERIOD=20000;

void initmotor()
{


    M1cw.period_us(256);
    M1ccw.period_us(256);
    M2cw.period_us(256);
    M2ccw.period_us(256);

}

void move(int left,int right)
{

    float rightduty,leftduty;

    if(right>256) {
        right=256;
    }
    if(left>256) {
        left=256;
    }
    if(right<-256) {
        right=-256;
    }
    if(left<-256) {
        left=-256;
    }

    rightduty=right/256.0;
    leftduty=left/256.0;
    if(right>0) {
        M1cw.write(1-rightduty);
        M1ccw.write(1);
    } else {
        M1cw.write(1);
        M1ccw.write(1+rightduty);
    }

    if(left>0) {
        M2cw.write(1-leftduty);
        M2ccw.write(1);
    } else {
        M2cw.write(1);
        M2ccw.write(1+leftduty);
    }
}


void close_hand(void){
    int i,degree;

   pwmhand.period_ms(20);        //20ms
   
       degree=175;

       i=500+degree*1900/180;
       pwmhand.write(i/PERIOD); 
       
       
}

void close_arm(void) {
    int i,degree;

   pwmarm.period_ms(20);        //20ms
   
       degree=160;

       i=500+degree*1900/180;
       pwmarm.write(i/PERIOD); 
       
       
}



void open_hand(void) {
    int i,degree;

   pwmhand.period_ms(20);        //20ms
   
       degree=90;

       i=500+degree*1900/180;
       pwmhand.write(i/PERIOD); 
       
       
}



void open_arm(void) {
    int i,degree;

   pwmarm.period_ms(20);        //20ms
   
       degree=10;

       i=500+degree*1900/180;
       pwmarm.write(i/PERIOD); 
       
       
}

int main(){
 move(-30,-30);
 while(1){
 open_arm();
 close_hand();
 move(1,1);
 led=1;
 wait(1);
 close_arm();
 open_hand();
 move(-255,-255);
 led=0;
 wait(1);
 }
 return 0;   
}