File content as of revision 0:0705184c2e58:

//Program untuk Arm dengan PID tetha
#include "mbed.h"
#include "Motor.h"
#include "encoderKRAI.h"
#include "millis.h"

encoderKRAI encoderPelontar (PC_15, PC_14, 538, encoderKRAI::X4_ENCODING);
Motor motorPelontar(PB_1, PB_2, PB_15);
DigitalIn butt (USER_BUTTON);
DigitalOut pneu[3] = {(PC_7), (PC_8), (PC_9)}; 
//pneu 0, 1 = pneu buat ambil shagai
//pneu 2 = pneu buat lempar shagai

Serial pc(USBTX, USBRX,115200);
double theta;
double pulse;
int kondisi=0, cp=0;
uint32_t millisx;
 

void hitungParameter();

int main(){
   
    theta=0.0;
    encoderPelontar.reset();
 
      pneu[0] = 1;
      pneu[1] = 1;
      pneu[2] = 0;
      
while (1){
    // 2, 4, 6 mati ketika 1 namun led nyala
    //0 tuh narik kedalem

   /* if (butt){
      pneu[0] = 0; //led 2
      pneu[1] = 1; //4
      pneu[2] = 1; //6
    }
      else {
      pneu[0] = 1;
      pneu[1] = 1;
      pneu[2] = 1;
          }

*/

if (!butt){
    cp = 1;
    wait(1);
} 
    
if(cp){
    pneu[0] = 1;
    pneu[2] = 0;
    while (theta<160.0){ 
        motorPelontar.speed(0.7);
        hitungParameter();
        }
    motorPelontar.brake(BRAKE_HIGH);
    wait (1);
    pneu[0] = 0;
    wait_ms (1000);
    
    while (theta>30.0){
        motorPelontar.speed(-0.2);
        hitungParameter();
        }
    motorPelontar.brake(BRAKE_HIGH);
    wait (1);
    pneu[2] = 1;
    wait (1);
    pneu[2] = 0;
    cp=0;
}
}
}

void hitungParameter(){
    pulse=(double) encoderPelontar.getPulses()*360/538;
    encoderPelontar.reset();
    theta+=pulse;
    pc.printf("theta = %.2f\t\n", theta );
}