Project

General

Profile

Revision 21bf5ee1

ID21bf5ee10f2873732c3a2a0bf9c837a7aba4c4dc
Parent beea48aa
Child 86196a57

Added by Thomas Mullins about 12 years ago

Added safety limits for the forklift potentiometer

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