Statistics
| Branch: | Revision:

root / forklift / code / forklift.c @ 0bd4bb32

History | View | Annotate | Download (2.89 KB)

1
/*
2
 * ARM -> us
3
 * -height setpoint
4
 *
5
 * us -> ARM
6
 * -tracking ID (same for all forklifts)
7
 * -serial number (unique to each forklift)
8
 * -current height
9
 * -current height setpoint
10
 * -payload weight
11
 *
12
 */
13
 
14
#include "twi.h"
15
#include "analog.h"
16
#include "motor.h"
17
#include <avr/io.h>
18
#include <avr/interrupt.h>
19

    
20
#define TRACKING_ID 0x41
21
#define SERIAL_NUMBER 0x12
22

    
23
// indicies for forklift internal data
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
31

    
32
#define FORKLIFT_DATA_LEN          7
33

    
34

    
35
#define PROP(X,Y,Z)                (((X)*(Y))/(Z))
36

    
37
uint8_t internal_index = 0;
38
uint8_t internal_data[] = {
39
  TRACKING_ID,
40
  SERIAL_NUMBER,
41
  0,
42
  0,
43
  0,
44
  0,
45
  150
46
};
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
}
58
void slave_rx(uint8_t* data, int len)
59
{
60
  if (len > 0 && data[0] < FORKLIFT_DATA_LEN)
61
  {
62
    internal_index = data[0];
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
        }
78
  }
79
}
80

    
81
void slave_tx(void)
82
{
83
  twi_transmit(&internal_data[internal_index], 1);
84
  internal_index++;
85
  if (internal_index >= FORKLIFT_DATA_LEN)
86
    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

    
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
}
110

    
111
int main()
112
{
113
  error = 0;
114
  i_term = 0;
115
  init_int0();
116
  sei();
117
  twi_attachSlaveRxEvent(slave_rx);
118
  twi_attachSlaveTxEvent(slave_tx);
119
  twi_setAddress(TRACKING_ID);
120
  twi_init();
121
  analog_init();
122
  motor_init();
123
  while (1)
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);
131
  }
132
  return 0;
133
}