commit 0bd4bb32e635bcc968c90d896920bf6618df9f48
Author: Tom Mullins <tmullins@andrew.cmu.edu>
Date:   Sun Mar 25 00:07:38 2012 -0400

    Added motor code for forklift, and fixed analog and i2c code after testing.

diff --git a/forklift/code/analog.c b/forklift/code/analog.c
index 44b4634..87687da 100644
--- a/forklift/code/analog.c
+++ b/forklift/code/analog.c
@@ -1,5 +1,7 @@
 #include <avr/io.h>
-#include "analog.h"
+#include "analog.h"
+
+int line_threshold = 150;
 
 int line_read(int which);
 void line_update(int* values);
@@ -60,7 +62,7 @@ void line_update(int* values)
 {
   int i;
   for(i = 0; i<5; i++)
-    values[i] = line_read(i) < 150 ? LWHITE : LBLACK;
+    values[i] = line_read(i) < line_threshold ? LWHITE : LBLACK;
 }
 
 int line_locate(int* values)
@@ -87,3 +89,13 @@ int line_read_pos(void)
   line_update(values);
   return line_locate(values);
 }
+
+void line_set_threshold_high(uint8_t threshold)
+{
+  line_threshold = ((int)(threshold & 3) << 8) | (line_threshold & 0xFF);
+}
+
+void line_set_threshold_low(uint8_t threshold)
+{
+  line_threshold = (line_threshold & 0x300) | (uint16_t) threshold;
+}
diff --git a/forklift/code/analog.h b/forklift/code/analog.h
index 98f303a..c1744c2 100644
--- a/forklift/code/analog.h
+++ b/forklift/code/analog.h
@@ -17,6 +17,8 @@
 
 void analog_init(void);
 int analog_read(int which);
-int line_read_pos(void);
+int line_read_pos(void);
+void line_set_threshold_high(uint8_t threshhold);
+void line_set_threshold_low(uint8_t threshhold);
 
 #endif
diff --git a/forklift/code/forklift.c b/forklift/code/forklift.c
index 355f8a8..039159e 100644
--- a/forklift/code/forklift.c
+++ b/forklift/code/forklift.c
@@ -12,19 +12,27 @@
  */
  
 #include "twi.h"
-#include "analog.h"
+#include "analog.h"
+#include "motor.h"
+#include <avr/io.h>
+#include <avr/interrupt.h>
 
 #define TRACKING_ID 0x41
 #define SERIAL_NUMBER 0x12
 
 // indicies for forklift internal data
-#define FORKLIFT_TRACKING_ID     0
-#define FORKLIFT_SERIAL_NUMBER   1
-#define FORKLIFT_HEIGHT          2
-#define FORKLIFT_HEIGHT_SETPOINT 3 // only one that's r/w
-#define FORKLIFT_LINEPOS         4
+#define FORKLIFT_TRACKING_ID       0
+#define FORKLIFT_SERIAL_NUMBER     1
+#define FORKLIFT_HEIGHT            2
+#define FORKLIFT_HEIGHT_SETPOINT   3 // r/w
+#define FORKLIFT_LINE_POS          4
+#define FORKLIFT_LINE_THRESH_HIGH  5 // r/w
+#define FORKLIFT_LINE_THRESH_LOW   6 // r/w
 
-#define FORKLIFT_DATA_LEN        5
+#define FORKLIFT_DATA_LEN          7
+
+
+#define PROP(X,Y,Z)                (((X)*(Y))/(Z))
 
 uint8_t internal_index = 0;
 uint8_t internal_data[] = {
@@ -32,16 +40,41 @@ uint8_t internal_data[] = {
   SERIAL_NUMBER,
   0,
   0,
-  0
+  0,
+  0,
+  150
 };
-
+int error;
+int i_term;
+
+
+void init_int0(void)
+{
+  TCCR0B = (1<<CS02)|(1<<CS00); //Timer clock = system clock / 1024
+  TIFR0 = 1<<TOV0; //Clear TOV0  clear pending interrupts
+  TIMSK0 = 1<<TOIE0; //Enable Timer0 Overflow Interrupt
+  
+}
 void slave_rx(uint8_t* data, int len)
 {
   if (len > 0 && data[0] < FORKLIFT_DATA_LEN)
   {
     internal_index = data[0];
-    if (len > 1 && internal_index == FORKLIFT_HEIGHT_SETPOINT)
-      internal_data[internal_index] = data[1];
+    if (len > 1)
+	{
+	  if (internal_index == FORKLIFT_HEIGHT_SETPOINT)
+        internal_data[internal_index] = data[1];
+      else if (internal_index == FORKLIFT_LINE_THRESH_HIGH)
+	  {
+	    internal_data[internal_index] = data[1];
+		line_set_threshold_high(data[1]);
+	  }
+      else if (internal_index == FORKLIFT_LINE_THRESH_LOW)
+	  {
+	    internal_data[internal_index] = data[1];
+		line_set_threshold_low(data[1]);
+	  }
+	}
   }
 }
 
@@ -52,19 +85,49 @@ void slave_tx(void)
   if (internal_index >= FORKLIFT_DATA_LEN)
     internal_index = 0;
 }
+
+void update_height(void)
+//Call to update the forklift height point. Should be called each loop
+{
+	internal_data[FORKLIFT_HEIGHT] = analog_read(ADC_HEIGHT);
+}
+
+
+int clamp(int min, int max, int val)
+{
+	if(val>max) return max;
+	if(val<min) return min;
+	return val;
+}
+
+SIGNAL(TIMER0_OVF_vect)
+{
+	error = (int)internal_data[FORKLIFT_HEIGHT]
+						-(int)internal_data[FORKLIFT_HEIGHT_SETPOINT];
+	i_term = i_term +  error/10;
+	set_motor(clamp(-127,127, i_term/10 + (error*2)/5));
+}
 
 int main()
-{
-  twi_init();
-  twi_setAddress(TRACKING_ID);
+{
+  error = 0;
+  i_term = 0;
+  init_int0();
+  sei();
   twi_attachSlaveRxEvent(slave_rx);
   twi_attachSlaveTxEvent(slave_tx);
-  analog_init();
+  twi_setAddress(TRACKING_ID);
+  twi_init();
+  analog_init();
+  motor_init();
   while (1)
-  {
-    // do motor stuff
-    internal_data[FORKLIFT_LINEPOS] = line_read_pos();
-    internal_data[FORKLIFT_HEIGHT] = analog_read(ADC_HEIGHT);
+  {
+  	/*error = PROP((int)internal_data[FORKLIFT_HEIGHT]
+						-(int)internal_data[FORKLIFT_HEIGHT_SETPOINT],2,5);
+		set_motor(clamp(-127,127,error));*/
+    // do motor stuff
+	internal_data[FORKLIFT_LINE_POS] = (analog_read(ADC_LINE)>>2);//line_read_pos();
+    internal_data[FORKLIFT_HEIGHT] = (analog_read(ADC_HEIGHT)>>2);
   }
   return 0;
 }
diff --git a/forklift/code/motor.c b/forklift/code/motor.c
new file mode 100644
index 0000000..065211c
--- /dev/null
+++ b/forklift/code/motor.c
@@ -0,0 +1,60 @@
+#include <avr/io.h>
+#include "motor.h"
+#include <stdlib.h>
+/*
+
+motor controller
+
+IN1: PD7
+IN2: PD6
+PWM: PB1
+
+
+*/
+
+int8_t motor_speed;
+
+
+
+void motor_init(void)
+{
+  // WGM1 0b0101 (fast PWM, 8-bit)
+  // COM1A 0b10 (clear OC1A on compare match, set on BOTTOM)
+  // CS1 0b011 (64 prescaler)
+  TCCR1A = _BV(WGM10) | _BV(COM1A1);
+  TCCR1B = _BV(WGM12) | _BV(CS11) | _BV(CS10);
+  OCR1AH = 0;
+  
+  // set IN1 (PD7), IN2 (PD6), and PWM (PB1) as output
+  DDRD |= _BV(DDD7) | _BV(DDD6);
+  DDRB |= _BV(DDB1);
+
+  set_motor(0);
+}
+
+void set_motor(int8_t speed)
+/* Speed must be between -127 and 127 */
+{
+  motor_speed = speed;
+  OCR1AL = abs(speed)*2;
+  if(speed>0) //go forwards
+  {
+  	PORTD |= (1<<PD7);
+	PORTD = PORTD & ~(1<<PD6);
+  }	
+  if(speed<0) //go backwards
+  {
+  	PORTD |= (1<<PD6);
+	PORTD = PORTD & ~(1<<PD7);
+  }
+  if(speed==0) // turn motor off
+  {
+	PORTD = PORTD & ~(1<<PD6);
+	PORTD = PORTD & ~(1<<PD7);
+  }
+}
+
+int8_t get_motor()
+{
+  return motor_speed;
+}
diff --git a/forklift/code/motor.h b/forklift/code/motor.h
new file mode 100644
index 0000000..3e949e6
--- /dev/null
+++ b/forklift/code/motor.h
@@ -0,0 +1,8 @@
+#ifndef _MOTOR_H_
+#define _MOTOR_H_
+
+void motor_init(void);
+void set_motor(int8_t speed);
+int8_t get_motor();
+
+#endif
