Newer
Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
#include "Controller.h"
#include <avr/io.h>
#include <avr/interrupt.h>
#include "servo.h"
Controller controller;
static servo_data blue_servo_data
{
220, //0°
2200, //180°
5000, //ICR_value
17366 //Pulse_duration
};
static usart_regs uart_regs
{
&UCSRA,
&UCSRB,
&UCSRC,
&UDR,
&UBRRL,
&UBRRH
};
Controller::Controller(): servo(&DDRB, PB1, &OCR1A, blue_servo_data),
adc(),
uart(uart_regs)
{
//Servo ini
TCCR1A |= (1<<COM1A1) | (1<<COM1B1) | (1<<WGM11); //Fast PWM non-inverted
TCCR1B |= (1<<WGM13) | (1<<WGM12) | (1<<CS11) | (1<<CS10); //Prescaler 64
ICR1 = 4000; //Top value
//UART ini
uart.set_operating_mode(uart.ASYNCHRONOUS_NORMAL);
uart.set_baudrate(9600);
uart.set_data_bits(8);
uart.set_stop_bits(1);
uart.enable_reciver();
uart.enable_trasmitter();
//ADC ini
DDRC = 0;
PORTC = 0;
ADCSRA = (1<<ADEN) | (1<<ADIE) | (1<<ADPS2) | (1<<ADPS1) | (1<<ADPS0);
//adc.set_reference_voltage(adc.INTERNAL_REFERENCE); //2.56V
//adc.set_prescaler(adc.PRESCALER_128); //115kHz
//adc.enable_interrupt();
//adc.set_channel(adc.ADC0);
//adc.enable_adc();
//TCC0 ini
TCCR0 = (1<<CS02); //225Hz
TIMSK |= (1<<TOIE0);
//Ext IRQ ini
MCUCR |= (1<<ISC11); //Falling edge
GICR |= (1<<INT1);
//TCC2 ini
TIMSK |= (1<<TOIE2);
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
void Controller::gate_handler()
{
switch (gate_mode)
{
case WAITING_FOR_LOCK_SIGNAL: waiting_for_lock_signal();
break;
case OPENING_PISTON: opening_piston();
break;
case WAITING_FOR_CLOSING_CURRENT: waiting_for_closing_current();
break;
case WAITING_FOR_CURRENT_REVERSATION: waiting_for_current_reversation();
break;
case WAITING_FOR_CURRENT_DROP: waiting_for_current_drop();
break;
case WAITING_FOR_STABILIZATION: waiting_for_stabilization();
break;
case CLOSING_PISTON: closing_piston();
break;
default:
break;
}
}
void Controller::waiting_for_lock_signal()
{
if (lock_signal)
{
gate_mode = OPENING_PISTON;
}
}
void Controller::opening_piston()
{
servo.set_angle(OPEN_POSITION);
gate_mode = WAITING_FOR_CLOSING_CURRENT;
}
void Controller::waiting_for_closing_current()
{
if ((current_negative > ON_TRESHOLD) && (current_negative < 1020))
}
}
void Controller::waiting_for_current_reversation()
{
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
{
gate_mode = WAITING_FOR_CURRENT_DROP;
}
}
void Controller::waiting_for_current_drop()
{
if ((current_positive < OFF_TRESHOLD) && (current_negative < OFF_TRESHOLD))
{
gate_mode = WAITING_FOR_STABILIZATION;
}
}
void Controller::waiting_for_stabilization()
{
if (stabilization_delay >= STABILIZATION_TRESHOLD)
{
stabilization_delay = 0;
gate_mode = CLOSING_PISTON;
}
}
void Controller::closing_piston()
{
servo.set_angle(CLOSE_POSITION);
gate_mode = WAITING_FOR_LOCK_SIGNAL;
}
void Controller::print_debug()
{
uart.transmit_string("P: ");
uart.transmit_dec32(current_positive);
uart.transmit_string("\tN: ");
uart.transmit_dec32(current_negative);
uart.transmit_string("\tL: ");
uart.transmit_dec32((char)lock_signal);
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
uart.transmit_string("\t M: ");
switch (gate_mode)
{
case WAITING_FOR_LOCK_SIGNAL: uart.transmit_string("Waiting for lock signal");
break;
case OPENING_PISTON: uart.transmit_string("Opening piston");
break;
case WAITING_FOR_CLOSING_CURRENT: uart.transmit_string("Waiting for closing current");
break;
case WAITING_FOR_CURRENT_REVERSATION: uart.transmit_string("Waiting for current reversation");
break;
case WAITING_FOR_CURRENT_DROP: uart.transmit_string("Waiting for current drop");
break;
case WAITING_FOR_STABILIZATION: uart.transmit_string("Waiting for stabilization");
break;
case CLOSING_PISTON: uart.transmit_string("Closing piston");
break;
default:
break;
}