Revision 0bd4bb32

View differences:

forklift/code/analog.c
1 1
#include <avr/io.h>
2
#include "analog.h"
2
#include "analog.h"
3

  
4
int line_threshold = 150;
3 5

  
4 6
int line_read(int which);
5 7
void line_update(int* values);
......
60 62
{
61 63
  int i;
62 64
  for(i = 0; i<5; i++)
63
    values[i] = line_read(i) < 150 ? LWHITE : LBLACK;
65
    values[i] = line_read(i) < line_threshold ? LWHITE : LBLACK;
64 66
}
65 67

  
66 68
int line_locate(int* values)
......
87 89
  line_update(values);
88 90
  return line_locate(values);
89 91
}
92

  
93
void line_set_threshold_high(uint8_t threshold)
94
{
95
  line_threshold = ((int)(threshold & 3) << 8) | (line_threshold & 0xFF);
96
}
97

  
98
void line_set_threshold_low(uint8_t threshold)
99
{
100
  line_threshold = (line_threshold & 0x300) | (uint16_t) threshold;
101
}
forklift/code/analog.h
17 17

  
18 18
void analog_init(void);
19 19
int analog_read(int which);
20
int line_read_pos(void);
20
int line_read_pos(void);
21
void line_set_threshold_high(uint8_t threshhold);
22
void line_set_threshold_low(uint8_t threshhold);
21 23

  
22 24
#endif
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
}
forklift/code/motor.c
1
#include <avr/io.h>
2
#include "motor.h"
3
#include <stdlib.h>
4
/*
5

  
6
motor controller
7

  
8
IN1: PD7
9
IN2: PD6
10
PWM: PB1
11

  
12

  
13
*/
14

  
15
int8_t motor_speed;
16

  
17

  
18

  
19
void motor_init(void)
20
{
21
  // WGM1 0b0101 (fast PWM, 8-bit)
22
  // COM1A 0b10 (clear OC1A on compare match, set on BOTTOM)
23
  // CS1 0b011 (64 prescaler)
24
  TCCR1A = _BV(WGM10) | _BV(COM1A1);
25
  TCCR1B = _BV(WGM12) | _BV(CS11) | _BV(CS10);
26
  OCR1AH = 0;
27
  
28
  // set IN1 (PD7), IN2 (PD6), and PWM (PB1) as output
29
  DDRD |= _BV(DDD7) | _BV(DDD6);
30
  DDRB |= _BV(DDB1);
31

  
32
  set_motor(0);
33
}
34

  
35
void set_motor(int8_t speed)
36
/* Speed must be between -127 and 127 */
37
{
38
  motor_speed = speed;
39
  OCR1AL = abs(speed)*2;
40
  if(speed>0) //go forwards
41
  {
42
  	PORTD |= (1<<PD7);
43
	PORTD = PORTD & ~(1<<PD6);
44
  }	
45
  if(speed<0) //go backwards
46
  {
47
  	PORTD |= (1<<PD6);
48
	PORTD = PORTD & ~(1<<PD7);
49
  }
50
  if(speed==0) // turn motor off
51
  {
52
	PORTD = PORTD & ~(1<<PD6);
53
	PORTD = PORTD & ~(1<<PD7);
54
  }
55
}
56

  
57
int8_t get_motor()
58
{
59
  return motor_speed;
60
}
forklift/code/motor.h
1
#ifndef _MOTOR_H_
2
#define _MOTOR_H_
3

  
4
void motor_init(void);
5
void set_motor(int8_t speed);
6
int8_t get_motor();
7

  
8
#endif

Also available in: Unified diff