Septian Dwi Herlangga Robotika

Soal 10

#include 
#include 

unsigned char l=0,r=0;
unsigned int sensors[5];
unsigned int position,battery,last_proportional=0;
long integral;

const char levels[] PROGMEM = {
 0b00000,
 0b00000,
 0b00000,
 0b00000,
 0b00000,
 0b00000,
 0b00000,
 0b01010,
 0b11111,
 0b11111,
 0b11111,
 0b01110,
 0b00100,
 0b00000,
 0b00000
};

void load_custom_characters(){
 lcd_load_custom_character(levels+0,0); 
 lcd_load_custom_character(levels+1,1); 
 lcd_load_custom_character(levels+2,2); 
 lcd_load_custom_character(levels+3,3);
 lcd_load_custom_character(levels+4,4);
 lcd_load_custom_character(levels+5,5);
 lcd_load_custom_character(levels+6,6);
 lcd_load_custom_character(levels+7,7);
 clear(); 
}

void display_readings(const unsigned int *calibrated_values){
 unsigned char i;

 for(i=0;i<5;i++) {  
  const char display_characters[11] = {' ',0,0,1,2,3,4,5,6,7,255};  
  char c = display_characters[calibrated_values[i]/101];
  print_character(c);
 }
}


void init(){
  unsigned char count;
  pololu_3pi_init(2000);
  load_custom_characters();
  while(!button_is_pressed(BUTTON_B))
   {
    battery = read_battery_millivolts();
 lcd_goto_xy(0,0);
 print_long(battery);
 delay_ms(50);
 print(" mV");
 lcd_goto_xy(0,1);
 print("Press B");
   }
  clear();
  
  lcd_goto_xy(0,0);
  print("Calibrat");
  lcd_goto_xy(0,1);
  print("ing");

  for(count=0; count<35; count++)
   {
    calibrate_line_sensors(IR_EMITTERS_ON);
//    if(count < 20 || count >= 59)
//  set_motors(-40,40);
// else
  set_motors(100,-100);
  delay_ms(10);
   }
   set_motors(0,0);
   clear();

  while(!button_is_pressed(BUTTON_B))
   {
    position = read_line(sensors,IR_EMITTERS_ON);
 lcd_goto_xy(0,0);
 print_long(position);
 lcd_goto_xy(0,1);
 display_readings(sensors);
 delay_ms(50);
 clear();
   }
  clear();
  lcd_goto_xy(0,0);
  print("Gooo!");
 }

volatile unsigned char spd;
void pid(){
  unsigned int position = read_line(sensors,IR_EMITTERS_ON);
  int proportional = ((int)position) - 2000;
 
  int derivative = proportional - last_proportional;
   integral += proportional;
   last_proportional = proportional;
   int power_difference = proportional/5 + integral/10000 + derivative*3/2;
   const int speed = spd;
     if(power_difference > speed)
   power_difference = speed;
  if(power_difference < -speed)
   power_difference = -speed;
  if(power_difference < 0)
   set_motors(speed + power_difference, speed);
  else
   set_motors(speed, speed - power_difference);
 }



//=================================================================
void kki(){
  while(1)
   {
    read_line(sensors,IR_EMITTERS_ON);
 pid();
    if(sensors[0] > 500 && sensors[1] > 500 && sensors[2] > 500)
     {
  play("L16af");
   l=1; r=0;
      break;
     }
   }
  delay_ms(50);
  set_motors(70,70);
  while(1) 
   {
    read_line(sensors,IR_EMITTERS_ON);
    if(sensors[0] < 200 && sensors[4] < 200)
     {
      break;
     }
   }
    set_motors(0,0);
 }

//-----------------------------------------------------------------
void kkn()
{
  while(1)
   {
    read_line(sensors,IR_EMITTERS_ON);
 pid();
    if(sensors[2] > 500 && sensors[3] > 500 && sensors[4] > 500)
     {
     play("L16af");
   r=1; l=0;
      break;
     }
   }
  set_motors(70,70);
  delay_ms(80);
  while(1) 
   {
    read_line(sensors,IR_EMITTERS_ON);
    if(sensors[0] < 200 && sensors[4] < 200)
     {
      break;
     }
   }
    set_motors(0,0);
 }

//-----------------------------------------------------------------
void ks(){
  while(1)
   {
    read_line(sensors,IR_EMITTERS_ON);
 pid();
    if(sensors[0] > 500 && sensors[1] > 500 && sensors[2] > 500 && sensors[3] > 500 && sensors[4] > 500)
     {
   play("L16af");
   l = r = 1;
      break;
     }
   }
  set_motors(70,70);
  while(1) 
   {
    read_line(sensors,IR_EMITTERS_ON);
    if(sensors[0] < 500 && sensors[4] < 500)
     {
   set_motors(0,0);
      break;
     }
   }
 }

//-----------------------------------------------------------------
void kp(){
  while(1)
   {
    read_line(sensors,IR_EMITTERS_ON);
 pid();
    if(sensors[0] < 100 && sensors[1] < 100 && sensors[2] < 100 && sensors[3] < 100 && sensors[4] < 100)
     {
   play("L16af");
      l = r = 0;
   break;
     }
   }
 }
//=================================================================
void mj(){
  set_motors(50,50);
  delay_ms(50);
  set_motors(0,0);}

//=================================================================
void maju(){
  set_motors(70,70);
  delay_ms(150);
//  while(1)
//   {
//    read_line(sensors,IR_EMITTERS_ON);
// if(sensors[4] < 200)
//  {
   set_motors(0,0);
//   break;
//  }
//   }
 }

//-----------------------------------------------------------------
void majuka(){
  set_motors(70,70);
  delay_ms(60);
  while(1)
   {
    read_line(sensors,IR_EMITTERS_ON);
 if(sensors[4] < 200)
  {
   set_motors(0,0);
   break;
  }
   }
 }

//-----------------------------------------------------------------
void majuki(){
  set_motors(70,70);
  delay_ms(60);
  while(1)
   {
    read_line(sensors,IR_EMITTERS_ON);
 if(sensors[0] < 200)
  {
   set_motors(0,0);
   break;
  }
   }
 }

//-----------------------------------------------------------------

void end(){
    set_motors(70,-70);
    play("L8acfgab");
    delay_ms(3000);
    set_motors(0,0);
    while(!button_is_pressed(BUTTON_B));
  }

//-----------------------------------------------------------------
void stop(){
  set_motors(0,0);
  l = r = 0;
 }
//=================================================================


//=================================================================
void ki(){
 while(1)
  {
  read_line(sensors,IR_EMITTERS_ON);
  set_motors(-70,70);
  if(sensors[2] < 100)
   {
    delay_ms(60);
    while(1)
        {
      read_line(sensors,IR_EMITTERS_ON);
      if(sensors[2] > 200)
        break;
     }
    break;
   }
 }
 set_motors(70,70);
 delay_ms(100); 
}


//-----------------------------------------------------------------
void kn(){
 while(1)
  {
   read_line(sensors,IR_EMITTERS_ON);
   set_motors(70,-70);
   if(sensors[2] < 100)
 {
     delay_ms(60);
   while(1)
       {
     read_line(sensors,IR_EMITTERS_ON);
     if(sensors[2] > 200)
      break;
    }
  break;
 }
  } 
  set_motors(70,70);
  delay_ms(100);
}

//-----------------------------------------------------------------
void kn2(){
 while(1)
  {
   read_line(sensors,IR_EMITTERS_ON);
   set_motors(70,-70);
   if(sensors[2]>500)
    {
     delay_ms(120);
  break;
    }
  }
 while(1)
  {
   read_line(sensors,IR_EMITTERS_ON);
   if(sensors[2]>500)
    {
     set_motors(0,0);
     break;
    }
  }
 set_motors(70,70);
 delay_ms(100); 
}

//-----------------------------------------------------------------
void ki3(){
 while(1)
  {
   read_line(sensors,IR_EMITTERS_ON);
   set_motors(-70,70);
   if(sensors[2]>500)
    {
     delay_ms(150);
  break;
    }
  }
 while(1)
  {
   read_line(sensors,IR_EMITTERS_ON);
   set_motors(-70,70);
   if(sensors[2]>500)
    {
     delay_ms(150);
  break;
    }
  }
 while(1)
  {
   read_line(sensors,IR_EMITTERS_ON);
   if(sensors[2]>500)
    {
     set_motors(0,0);
     break;
    }
  }
 set_motors(70,70);
 delay_ms(100); 
}

//-----------------------------------------------------------------
void kn3(){
 while(1)
  {
   read_line(sensors,IR_EMITTERS_ON);
   set_motors(70,-70);
   if(sensors[2]>500)
    {
     delay_ms(100);
  break;
    }
  }
 while(1)
  {
   read_line(sensors,IR_EMITTERS_ON);
   set_motors(70,-70);
   if(sensors[2]>500)
    {
     delay_ms(100);
  break;
    }
  }
 while(1)
  {
   read_line(sensors,IR_EMITTERS_ON);
   if(sensors[2]>500)
    {
     set_motors(0,0);
     break;
    }
  }
 set_motors(70,70);
 delay_ms(100); 
}

//-----------------------------------------------------------------
void ki2(){
 while(1)
  {
   read_line(sensors,IR_EMITTERS_ON);
   set_motors(-70,70);
   if(sensors[2]>500)
    {
     delay_ms(120);
  break;
    }
  }
 while(1)
  {
   read_line(sensors,IR_EMITTERS_ON);
   if(sensors[2]>500)
    {
     set_motors(0,0);
     break;
    }
  }
 set_motors(70,70);
 delay_ms(100); 
}

void u()
 {
  if(l==1 && r==1)
   {
    l=r=0;
 kn(); kn2();
   }
  else if(l==0 && r==1)
   {
    l=r=0;
 ki();
   }
  else if(l==1 && r==0)
   {
    l=r=0;
 kn();
   }
  else
   {
    kn();
   }
 }
//-----------------------------------------------------------------
void u2()
 {
  if(l==1 && r==1)
   {
    l=r=0;
 kn(); kn2();
   }
  else if(l==0 && r==1)
   {
    l=r=0;
 set_motors(-70,70);
 delay_ms(150);
 ki();
   }
  else if(l==1 && r==0)
   {
    l=r=0;
 set_motors(70,-70);
 delay_ms(150);
 kn();
   }
  else
   {
    kn();
   }  
 }
//-----------------------------------------------------------------
void go_timeout(unsigned int waktu){
  unsigned int tick;
  for(tick = 0; tick < waktu; tick++)
   {
    pid();
   }
  set_motors(0,0);
 }
 
//-----------------------------------------------------------------
void ka(){
  while(1)
   {
    read_line(sensors,IR_EMITTERS_ON);
 pid();
    if(sensors[0] > 500 || sensors[4] > 500)
     {
   play("L16af");
   l = r = 1;
      break;
     }
   }
  set_motors(70,70);
  delay_ms(160);
//  while(1) 
//   {
//    read_line(sensors,IR_EMITTERS_ON);
//    if(sensors[1] < 400 || sensors[3] < 400)
//     {
   set_motors(0,0);
//      break;
//     }
//   }
 }

void ks2(){
  while(1)
   {
    read_line(sensors,IR_EMITTERS_ON);
 pid();
    //if(sensors[0] > 200 && sensors[1] > 200 && sensors[2] > 200 && sensors[3] > 200 && sensors[4] > 200)
 if(sensors[0] > 200 || sensors[4] > 200)
     {
   l = r = 1;
   set_motors(0,0);
      break;
     }
   }
}

void ky()
 {
  while(1)
   {
    read_line(sensors,IR_EMITTERS_ON);
 pid();
 if(sensors[0] > 500 && sensors[1] > 500 && sensors[1] > 500)
  {
   play("L16af");
   break;
  }
   }
  set_motors(70,70);
  while(1)
   {
    read_line(sensors,IR_EMITTERS_ON);
    if(sensors[0] < 200 && sensors[4] < 200)
  {
   set_motors(0,0);
   break;
  }
   }
 }

//================================================================
int main(){
init();
spd = 200;


while(1)
  {
ks();
maju();
kki();
ki();
kki();
u();
kkn();
kn();
ks();
end();
stop();
 }
}
//================================================================