Revision 21bf5ee1 forklift/code/forklift.c

View differences:

forklift/code/forklift.c
68 68
  {
69 69
    internal_index = data[0];
70 70
    if (len > 1)
71
	{
72
	  if (internal_index == FORKLIFT_HEIGHT_SETPOINT)
71
  {
72
    if (internal_index == FORKLIFT_HEIGHT_SETPOINT)
73 73
        internal_data[internal_index] = data[1];
74 74
      else if (internal_index == FORKLIFT_LINE_THRESH_HIGH)
75
	  {
76
	    internal_data[internal_index] = data[1];
77
		line_set_threshold_high(data[1]);
78
	  }
75
    {
76
      internal_data[internal_index] = data[1];
77
    line_set_threshold_high(data[1]);
78
    }
79 79
      else if (internal_index == FORKLIFT_LINE_THRESH_LOW)
80
	  {
81
	    internal_data[internal_index] = data[1];
82
		line_set_threshold_low(data[1]);
83
	  }
84
	}
80
    {
81
      internal_data[internal_index] = data[1];
82
    line_set_threshold_low(data[1]);
83
    }
84
  }
85 85
  }
86 86
}
87 87

  
......
95 95

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

  
103 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));
104
{
105
  // TODO make this more easily tunable (despite integer limitations)
106
  int height = (int)internal_data[FORKLIFT_HEIGHT];
107
  error = (int)internal_data[FORKLIFT_HEIGHT_SETPOINT] - height;
108
  i_term = i_term + error/10;
109
  int speed = clamp(-127, 127, i_term/10 + (error*2)/5);
110
  if (height < 10 && speed < 0) speed = 0;
111
  if (height > 245 && speed > 0) speed = 0;
112
  set_motor(speed);
109 113
}
110 114

  
111 115
int main()
......
124 128
  while (1)
125 129
  {
126 130
    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();
131
      internal_data[i + FORKLIFT_LINE_VALS_START] = line_read(i) >> 2;
132
    internal_data[FORKLIFT_LINE_POS] = line_read_pos();
129 133
    internal_data[FORKLIFT_HEIGHT] = analog_read(ADC_HEIGHT) >> 2;
130 134
  }
131 135
  return 0;

Also available in: Unified diff