File content as of revision 0:9dfae2e18414:

#include "mbed.h"
#include "Servo.h"
#include "Motor.h"

/* Nama servo
    4
    3
  21 22
    1 */
    
Servo servo1(PA_1);
Servo servo21(PA_2);
Servo servo22(PA_3);
Servo servo3(PA_4);
Servo servo4(PA_5);
Motor motorKanan(PB_3, PA_10, PC_4);
Motor motorKiri(PB_7, PC_3, PC_0);

void kalibrasi(float deg){
    servo1.calibrate(0.0005,deg);
    servo21.calibrate(0.0005,deg);
    servo22.calibrate(0.0005,deg);
    servo3.calibrate(0.0005,deg);
    servo4.calibrate(0.0005,deg);
}

void idle(float s1,float s21,float s22, float s3, float s4){
    servo1.position(s1);
    wait_ms(1);
    servo21.position(s21);
    servo22.position(s22);
    wait_ms(1);
    servo3.position(s3);
    wait_ms(1);
    servo4.position(s4);
    wait_ms(1);
}

int main() {
    /***Gerak Jalan***
    //Maju
    motorKanan.speed(0.3);
    motorKiri.speed(0.3);
    
    //Mundur
    motorKanan.speed(0.3);
    motorKiri.speed(0.3);
    
    //PivotKiri
    motorKanan.brake();
    motorKiri.speed(0.3);
    
    //PivotKanan
    motorKiri.brake();
    motorKanan.speed(0.3);
    */
    
    /***Gerak Tangan***/
    //kalibrasi
    kalibrasi(300);
    
    //mode1(idle)
    idle(0.0,90.0,90.0,90.0,0.0);
    
    //mode2(gerakan keluar)
}