Revision beea48aa forklift/code/forklift.c

View differences:

forklift/code/forklift.c
27 27
#define FORKLIFT_HEIGHT_SETPOINT   3 // r/w
28 28
#define FORKLIFT_LINE_POS          4
29 29
#define FORKLIFT_LINE_THRESH_HIGH  5 // r/w
30
#define FORKLIFT_LINE_THRESH_LOW   6 // r/w
30
#define FORKLIFT_LINE_THRESH_LOW   6 // r/w
31
#define FORKLIFT_LINE_VALS_START   7
32
#define FORKLIFT_LINE_VALS_END    12 // non-inclusive
31 33

  
32
#define FORKLIFT_DATA_LEN          7
34
#define FORKLIFT_DATA_LEN         12
33 35

  
34 36

  
35 37
#define PROP(X,Y,Z)                (((X)*(Y))/(Z))
......
42 44
  0,
43 45
  0,
44 46
  0,
45
  150
47
  150, // default line threshold
48
  0,
49
  0,
50
  0,
51
  0,
52
  0
46 53
};
47 54
int error;
48 55
int i_term;
......
84 91
  internal_index++;
85 92
  if (internal_index >= FORKLIFT_DATA_LEN)
86 93
    internal_index = 0;
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
}
94 95

  
95

  
96 96
int clamp(int min, int max, int val)
97 97
{
98 98
	if(val>max) return max;
......
105 105
	error = (int)internal_data[FORKLIFT_HEIGHT]
106 106
						-(int)internal_data[FORKLIFT_HEIGHT_SETPOINT];
107 107
	i_term = i_term +  error/10;
108
	set_motor(clamp(-127,127, i_term/10 + (error*2)/5));
108
	//set_motor(clamp(-127,127, i_term/10 + (error*2)/5));
109 109
}
110 110

  
111 111
int main()
112 112
{
113
  int i;
113 114
  error = 0;
114 115
  i_term = 0;
115 116
  init_int0();
......
122 123
  motor_init();
123 124
  while (1)
124 125
  {
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);
126
    for (i = 0; i < 5; i++)
127
  	  internal_data[i + FORKLIFT_LINE_VALS_START] = line_read(i) >> 2;
128
	internal_data[FORKLIFT_LINE_POS] = line_read_pos();
129
    internal_data[FORKLIFT_HEIGHT] = analog_read(ADC_HEIGHT) >> 2;
131 130
  }
132 131
  return 0;
133 132
}

Also available in: Unified diff