Project

General

Profile

Revision beea48aa

IDbeea48aaab7b027978a3703b75fe90f29e6ddc2f

Added by Thomas Mullins about 12 years ago

Tested forklift code and fixed some things (a few weeks ago)

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