File content as of revision 0:b5d91230ff3c:

#include "mbed.h"
#include "TB6612.h"
#include "HighSpeedAnalogIn.h"

#define SENSOR_BORDER   1000
#define SENSOR_NUM      4
#define MOTOR_MAX_VALUE 100
#define MOTOR_MIN_VALUE -100

#define KP 1
#define KI 0
#define KD 20

BusOut sensor_on(LED1, LED2, LED3, LED4);

TB6612 left(p21,p12,p11);
TB6612 right(p22,p14,p13);

HighSpeedAnalogIn sensor(p15, p16, p17, p18, p19, p20);

DigitalIn sw(p29);
Ticker tick;

int motor_cnt_val = 0;

void cycle()
{
  float sensor_val = 0;
  float p_val,d_val,pid_val;
  static float i_val,prev_sensor_val = 0;
  int bit = 0;

  if( sensor.read_u16(p15) > SENSOR_BORDER ) bit |= 0x01;
  if( sensor.read_u16(p16) > SENSOR_BORDER ) bit |= 0x02;
  if( sensor.read_u16(p17) > SENSOR_BORDER ) bit |= 0x04;
  if( sensor.read_u16(p18) > SENSOR_BORDER ) bit |= 0x08;

  sensor_on = bit;

  switch(bit)
  {
  case 0x01:    sensor_val = -1.0;   break;
  case 0x03:    sensor_val = -0.66;  break;
  case 0x02:    sensor_val = -0.33;  break;
  case 0x04:    sensor_val = 0.33;   break;
  case 0x0C:    sensor_val = 0.66;   break;
  case 0x08:    sensor_val = 1.0;    break;
  default:      sensor_val = 0;      break;
  }
  
  p_val = sensor_val;
  i_val += p_val;
  d_val = sensor_val - prev_sensor_val;
  prev_sensor_val = sensor_val;
  
  pid_val = (p_val * (KP) ) + (i_val*(KI)) + (d_val*(KD));
  
  motor_cnt_val = (int)(pid_val * 100.0);
}

int main() {
  sw.mode(PullUp);
  
  tick.attach(cycle,0.01);

  while(sw==1)
  {
    int a[4] = {sensor.read_u16(p15),sensor.read_u16(p16),sensor.read_u16(p17),sensor.read_u16(p18)};
    
    printf("[0]:%05d  [0]:%05d  [0]:%05d  [0]:%05d\n\r",a[0],a[1],a[2],a[3]);
    wait(0.1);
  }

  wait(1);

  while(1) {

    if( motor_cnt_val < 0 )
    {
      right = MOTOR_MAX_VALUE
              + motor_cnt_val;
    }
    else
    {
      right = MOTOR_MAX_VALUE;
    }
    if( motor_cnt_val > 0 )
    {
      left = MOTOR_MAX_VALUE
             - motor_cnt_val;
    }
    else
    {
      left = MOTOR_MAX_VALUE;
    }
  }
}