Revision 0bd4bb32 forklift/code/forklift.c

View differences:

forklift/code/forklift.c
12 12
 */
13 13
 
14 14
#include "twi.h"
15
#include "analog.h"
15
#include "analog.h"
16
#include "motor.h"
17
#include <avr/io.h>
18
#include <avr/interrupt.h>
16 19

  
17 20
#define TRACKING_ID 0x41
18 21
#define SERIAL_NUMBER 0x12
19 22

  
20 23
// indicies for forklift internal data
21
#define FORKLIFT_TRACKING_ID     0
22
#define FORKLIFT_SERIAL_NUMBER   1
23
#define FORKLIFT_HEIGHT          2
24
#define FORKLIFT_HEIGHT_SETPOINT 3 // only one that's r/w
25
#define FORKLIFT_LINEPOS         4
24
#define FORKLIFT_TRACKING_ID       0
25
#define FORKLIFT_SERIAL_NUMBER     1
26
#define FORKLIFT_HEIGHT            2
27
#define FORKLIFT_HEIGHT_SETPOINT   3 // r/w
28
#define FORKLIFT_LINE_POS          4
29
#define FORKLIFT_LINE_THRESH_HIGH  5 // r/w
30
#define FORKLIFT_LINE_THRESH_LOW   6 // r/w
26 31

  
27
#define FORKLIFT_DATA_LEN        5
32
#define FORKLIFT_DATA_LEN          7
33

  
34

  
35
#define PROP(X,Y,Z)                (((X)*(Y))/(Z))
28 36

  
29 37
uint8_t internal_index = 0;
30 38
uint8_t internal_data[] = {
......
32 40
  SERIAL_NUMBER,
33 41
  0,
34 42
  0,
35
  0
43
  0,
44
  0,
45
  150
36 46
};
37

  
47
int error;
48
int i_term;
49

  
50

  
51
void init_int0(void)
52
{
53
  TCCR0B = (1<<CS02)|(1<<CS00); //Timer clock = system clock / 1024
54
  TIFR0 = 1<<TOV0; //Clear TOV0  clear pending interrupts
55
  TIMSK0 = 1<<TOIE0; //Enable Timer0 Overflow Interrupt
56
  
57
}
38 58
void slave_rx(uint8_t* data, int len)
39 59
{
40 60
  if (len > 0 && data[0] < FORKLIFT_DATA_LEN)
41 61
  {
42 62
    internal_index = data[0];
43
    if (len > 1 && internal_index == FORKLIFT_HEIGHT_SETPOINT)
44
      internal_data[internal_index] = data[1];
63
    if (len > 1)
64
	{
65
	  if (internal_index == FORKLIFT_HEIGHT_SETPOINT)
66
        internal_data[internal_index] = data[1];
67
      else if (internal_index == FORKLIFT_LINE_THRESH_HIGH)
68
	  {
69
	    internal_data[internal_index] = data[1];
70
		line_set_threshold_high(data[1]);
71
	  }
72
      else if (internal_index == FORKLIFT_LINE_THRESH_LOW)
73
	  {
74
	    internal_data[internal_index] = data[1];
75
		line_set_threshold_low(data[1]);
76
	  }
77
	}
45 78
  }
46 79
}
47 80

  
......
52 85
  if (internal_index >= FORKLIFT_DATA_LEN)
53 86
    internal_index = 0;
54 87
}
88

  
89
void update_height(void)
90
//Call to update the forklift height point. Should be called each loop
91
{
92
	internal_data[FORKLIFT_HEIGHT] = analog_read(ADC_HEIGHT);
93
}
94

  
95

  
96
int clamp(int min, int max, int val)
97
{
98
	if(val>max) return max;
99
	if(val<min) return min;
100
	return val;
101
}
102

  
103
SIGNAL(TIMER0_OVF_vect)
104
{
105
	error = (int)internal_data[FORKLIFT_HEIGHT]
106
						-(int)internal_data[FORKLIFT_HEIGHT_SETPOINT];
107
	i_term = i_term +  error/10;
108
	set_motor(clamp(-127,127, i_term/10 + (error*2)/5));
109
}
55 110

  
56 111
int main()
57
{
58
  twi_init();
59
  twi_setAddress(TRACKING_ID);
112
{
113
  error = 0;
114
  i_term = 0;
115
  init_int0();
116
  sei();
60 117
  twi_attachSlaveRxEvent(slave_rx);
61 118
  twi_attachSlaveTxEvent(slave_tx);
62
  analog_init();
119
  twi_setAddress(TRACKING_ID);
120
  twi_init();
121
  analog_init();
122
  motor_init();
63 123
  while (1)
64
  {
65
    // do motor stuff
66
    internal_data[FORKLIFT_LINEPOS] = line_read_pos();
67
    internal_data[FORKLIFT_HEIGHT] = analog_read(ADC_HEIGHT);
124
  {
125
  	/*error = PROP((int)internal_data[FORKLIFT_HEIGHT]
126
						-(int)internal_data[FORKLIFT_HEIGHT_SETPOINT],2,5);
127
		set_motor(clamp(-127,127,error));*/
128
    // do motor stuff
129
	internal_data[FORKLIFT_LINE_POS] = (analog_read(ADC_LINE)>>2);//line_read_pos();
130
    internal_data[FORKLIFT_HEIGHT] = (analog_read(ADC_HEIGHT)>>2);
68 131
  }
69 132
  return 0;
70 133
}

Also available in: Unified diff