Diff: main.cpp

Revision:

0:b5d91230ff3c

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sat Aug 10 04:13:15 2013 +0000
@@ -0,0 +1,97 @@
+#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;
+    }
+  }
+}