File content as of revision 0:aa2dd4e13c71:

#include"mbed.h"
#include "stdio.h"
#include "Serial.h"
#include "CMPS03.h"  

/////////////////////motor///////////////////////
DigitalOut dir3(PB_9);
DigitalOut dir4(PB_8);
DigitalOut dir5(PC_5);
DigitalOut dir6(PC_8);
DigitalOut dir1(PA_12);
DigitalOut dir2(PB_12);
DigitalOut dir7(PB_14);
DigitalOut dir8(PB_15);
PwmOut pwm4(PB_1);
PwmOut pwm3(PC_9);
PwmOut pwm2(PC_6);
PwmOut pwm1(PA_11);

////////////////////////////////////////////////





float roda1, roda2, roda3, roda4;
float pw1,pw2,pw3, pw4;
float abs_pwm1, abs_pwm2, abs_pwm3, abs_pwm4;
double dx, dy, dz;

//////////////////////inverse///////////////////////

    
void atur(float x, float  y, float z)
{
                                                           
  roda1 = x - z + y;    
  roda2 = y - z + x;
  roda3 = y + z - x;
  roda4 = x + z - y;
    
  if (roda1 < 0) //kiri atas
  {
    abs_pwm1 = abs(roda1);
    pw1=(float) abs_pwm1/255;
    dir1 = 1;
    dir2 = 0;
  }
  else 
  {
    abs_pwm1 = roda1;
    pw1=(float) abs_pwm1/255;
    dir1 = 0;
    dir2 = 1;
  }
  if (roda2 < 0) // kanan atas
  {
    abs_pwm2 = abs(roda2);
    pw2=(float) abs_pwm2/255;
    dir3 = 0;
    dir4 = 1;
  }
  else 
  {
    abs_pwm2 = roda2;
    pw2=(float) abs_pwm2/255;
    dir3 = 1;
    dir4 = 0;
  }
  if (roda3 < 0) // kanan bawah
  {
    abs_pwm3 = abs(roda3);
    pw3=(float) abs_pwm3/255;
    dir5 = 1;
    dir6 = 0;
  }
  else 
  {
    abs_pwm3 = roda3;
    pw3=(float) abs_pwm3/255;
    dir5 = 0;
    dir6 = 1;
  }
  if (roda4 < 0) // kiri bawah
   {
    abs_pwm4 = abs(roda4);
    pw4=  ((float) abs_pwm4/255);
    dir7 = 1;
    dir8 = 0;
  }
  else 
  {
    abs_pwm4 = roda4;
    pw4=((float) abs_pwm4/255);
    dir7 = 0;
    dir8 = 1;
  }
    pwm1.write(pw1);
    pwm2.write(pw2);
    pwm3.write(pw3);
    pwm4.write(pw4);
}

////////////////////////////////////////////////


Serial pc(USBTX, USBRX);
CMPS03 compass(PB_4,PA_8 , CMPS03_DEFAULT_I2C_ADDRESS);
Serial device(PB_6, PB_7);
PinName pin_SERVO0  = PB_0;
PinName pin_SERVO1  = PB_3;
PinName pin_SERVO2  = PA_0;
PinName pin_SERVO3  = PA_10;
PinName pin_SERVO4  = PA_5;
PinName pin_SERVO5  = PA_7;
PinName pin_SERVO6  = PA_1;
PinName pin_SERVO7  = PB_10;
PinName pin_SERVO8  = PB_5;





 DigitalOut relay1(PA_13);
PwmOut Servo0(pin_SERVO0);
PwmOut Servo1(pin_SERVO1);
PwmOut Servo2(pin_SERVO2);
PwmOut Servo3(pin_SERVO3);
PwmOut Servo4(pin_SERVO4);
PwmOut Servo5(pin_SERVO5);
PwmOut Servo6(pin_SERVO6);
PwmOut Servo7(pin_SERVO7);
PwmOut Servo8(pin_SERVO8);

void Servo_angle(int16_t angle)
{
    int16_t Angle = 600 + (angle * 10);
    Servo0.pulsewidth_us(Angle);
    Servo1.pulsewidth_us(Angle);
    Servo2.pulsewidth_us(Angle);
    Servo3.pulsewidth_us(Angle);
    Servo4.pulsewidth_us(Angle);
    Servo5.pulsewidth_us(Angle);
    Servo6.pulsewidth_us(Angle);
    Servo7.pulsewidth_us(Angle);
    Servo8.pulsewidth_us(Angle);

}
 

 
int inputdata;
int data;
int data2;
double posisi;
int main() 
{
    device.baud(38400);
    pc.baud(57600); 
    Servo0.period_ms(20);   
    Servo1.period_ms(20);
    Servo2.period_ms(20);
    Servo3.period_ms(20);
    Servo4.period_ms(20);
    Servo5.period_ms(20);
    Servo6.period_ms(20);
    Servo7.period_ms(20);
    Servo8.period_ms(20);

    while(1) {
  
       

        if(device.readable()>0)
        { 
        inputdata = device.getc();
        data=inputdata;   
        pc.putc(inputdata);
        {
                
                
                
                       
                //penanaman
                if(data == 'i') //tutup
                {

                    // 180 ~ 0
                    for(int i=180; i>=0; i--)
                    {
                        int16_t Angle = 600 + (i * 10);
                        Servo2.pulsewidth_us(Angle);
                        Servo3.pulsewidth_us(Angle);
                        Servo4.pulsewidth_us(Angle);
                        Servo7.pulsewidth_us(Angle);

                    }
                }
                if(data == 'h') //buka
                {

                    // 180 ~ 0
                    for(int i=0; i<=110; i++)
                    {
                        int16_t Angle = 600 + (i * 10);
                        Servo2.pulsewidth_us(Angle);
                        Servo3.pulsewidth_us(Angle);
                        Servo4.pulsewidth_us(Angle);
                        Servo7.pulsewidth_us(Angle);

                    }
                }
                
                
                if(data == 'g') //arm naik 
                {

                    // 180 ~ 0
                    for(int i=180; i>=100; i--)
                    {
                        int16_t Angle = 600 + (i * 10);
                        Servo0.pulsewidth_us(Angle);
                        Servo1.pulsewidth_us(Angle);
                        Servo5.pulsewidth_us(Angle);
                        Servo6.pulsewidth_us(Angle);
                        Servo8.pulsewidth_us(Angle);

                    }
                }
                
                if(data == 'f') //arm turun
                {
                    // 0 ~ 180
                    for(int i=100; i<=180; i++)
                    {
                        int16_t Angle = 600 + (i * 10);
                        Servo0.pulsewidth_us(Angle);
                        Servo1.pulsewidth_us(Angle);
                        Servo5.pulsewidth_us(Angle);
                        Servo6.pulsewidth_us(Angle);
                        Servo8.pulsewidth_us(Angle);


                    }
                }
                // penyiangan
                if(data == 'b') //arm turun
                {
                    // 0 ~ 180
                    for(int i=100; i<=180; i++)
                    {
                        int16_t Angle = 600 + (i * 10);
                        Servo0.pulsewidth_us(Angle);
                        //Servo1.pulsewidth_us(Angle);
                        Servo5.pulsewidth_us(Angle);

                    }
                }
                if(data == 'c') //arm naik 
                {

                    // 180 ~ 0
                    for(int i=180; i>=100; i--)
                    {
                        int16_t Angle = 600 + (i * 10);
                        Servo0.pulsewidth_us(Angle);
                        //Servo1.pulsewidth_us(Angle);
                        Servo5.pulsewidth_us(Angle);
                    }
                }
                if(data == 'e') //tutup
                {

                    // 180 ~ 0
                    for(int i=180; i>=0; i--)
                    {
                        int16_t Angle = 600 + (i * 10);
                        Servo2.pulsewidth_us(Angle);
                        //Servo3.pulsewidth_us(Angle);
                        Servo4.pulsewidth_us(Angle);

                    }
                }
                if(data == 'd') //buka
                {

                    // 180 ~ 0
                    for(int i=0; i<=110; i++)
                    {
                        int16_t Angle = 600 + (i * 10);
                        Servo2.pulsewidth_us(Angle);
                        //Servo3.pulsewidth_us(Angle);
                        Servo4.pulsewidth_us(Angle);

                    }
                }
                
                 
                switch(data){
                    case'2':           
                        atur(50,0,0); //maju
                        break;
                    case'3':
                        atur(-50,0,0); //mundur
                        break;
                    case'7':
                        atur(0,-50,0); //kanan
                        break;
                    case'1':
                        atur(0,50,0); //kiri
                        break;
                    case'5':
                        atur(0,0,50); //muter kanan
                        break;
                    case'8':
                        atur(0,0,-50); //muter kiri
                        break;
                    case'9':
                    {
                    for(double posisi=0; posisi<=329.700012; posisi++)
                            {
                            posisi=compass.readBearing()/10.0;
                            printf("Bearing is: %f\n", posisi);
                            atur(0,0,100);
                            if(posisi==329.700012);
                    }
                    atur(0,0,0);
                
                
                    default:
                    dir1 = 1;
                    dir2 = 1;
                    dir3 = 1;
                    dir4 = 1;
                    dir5 = 1;
                    dir6 = 1;
                    dir7 = 1;
                    dir8 = 1;
                    ;    
                   

                }
                /*
                if(data == '3') //mundur
                {
                    atur(100,0,0);
                }
                
                if(data == '7') //kanan
                {
                    atur(0,100,0);
                }
                
                if(data == '1') //kiri
                {
                    atur(0,-100,0);
                }
                if(data == '4') //armkanan
                {
                    atur(0,0,150);
                }
                if(data == '8') //armkanan
                {
                    atur(0,0,-150);
                }
                if(data == 'r') //armkanan
                {
                    atur(0,0,0);
                }
                */
 
                
                // untuk mode pemotong menggunakan relay 
                 if(data == 'a'){ 
                relay1=1;
                
                }
                if(data == 'z') {
                    relay1=0;
                    }
                
                
                
                }
                
                

}}}}