354 lines
6.4 KiB
C
354 lines
6.4 KiB
C
#define F_CPU 3333333
|
|
#define USART0_BAUD_RATE(BAUD_RATE) ((float)(3333333 * 64 / (16 * (float)BAUD_RATE)) + 0.5)
|
|
#include <avr/io.h>
|
|
#include <stdbool.h>
|
|
#include <util/delay.h>
|
|
#include <string.h>
|
|
#include <stdlib.h>
|
|
#include <stdio.h>
|
|
#include "helpers_uart.h"
|
|
#include <avr/interrupt.h>
|
|
#include <xc.h>
|
|
#include "twi0_master_example.h"
|
|
|
|
#define LISTEN_EXPANDER_COMMANDS true
|
|
#define IGNORE_SOFT_LIMITS false
|
|
|
|
#define delay_time 50
|
|
|
|
// Zoom -> motor 0
|
|
// Focus -> motor 1
|
|
// Iris -> motor 2
|
|
struct PORT_struct *port_1[] = {&PORTB, &PORTA, &PORTC};
|
|
struct PORT_struct *port_2[] = {&PORTB, &PORTA, &PORTC};
|
|
struct PORT_struct *port_3[] = {&PORTC, &PORTA, &PORTC};
|
|
struct PORT_struct *port_4[] = {&PORTC, &PORTA, &PORTC};
|
|
|
|
uint8_t pin_1[] = {5, 2, 4};
|
|
uint8_t pin_2[] = {4, 3, 5};
|
|
uint8_t pin_3[] = {0, 4, 6};
|
|
uint8_t pin_4[] = {1, 5, 7};
|
|
|
|
uint8_t delay[] = {15, 15, 50}; // Multiples of "delay_time"
|
|
uint16_t max_steps[] = {2994, 5180, 77};
|
|
int16_t current_step[] = {0, 0, 0};
|
|
|
|
uint8_t enable_port[] = {2, 3, 4};
|
|
|
|
bool motor_max_pos;
|
|
bool motor_min_pos;
|
|
uint8_t phase;
|
|
|
|
uint8_t motor_num;
|
|
uint8_t port_val;
|
|
char usart_recv_char;
|
|
bool usart_interrupt = false;
|
|
|
|
|
|
uint8_t read_low_usart_bit(){
|
|
while (!(USART0.STATUS & USART_RXCIF_bm)) {
|
|
}
|
|
return USART0.RXDATAL;
|
|
}
|
|
uint8_t read_high_usart_bit(){
|
|
while (!(USART0.STATUS & USART_RXCIF_bm)) {
|
|
}
|
|
return USART0.RXDATAH;
|
|
}
|
|
|
|
|
|
ISR(USART0_RXC_vect)
|
|
{
|
|
|
|
|
|
usart_recv_char = read_low_usart_bit();
|
|
USART0_sendChar(usart_recv_char);
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
void set_ports_out()
|
|
{
|
|
for (int i = 0; i<sizeof(pin_1); i++) {
|
|
port_1[i]->DIRSET = _BV(pin_1[i]);
|
|
port_2[i]->DIRSET = _BV(pin_2[i]);
|
|
port_3[i]->DIRSET = _BV(pin_3[i]);
|
|
port_4[i]->DIRSET = _BV(pin_4[i]);
|
|
|
|
port_1[i]->OUTSET = _BV(pin_1[i]);
|
|
port_2[i]->OUTSET = _BV(pin_2[i]);
|
|
port_3[i]->OUTSET = _BV(pin_3[i]);
|
|
port_4[i]->OUTSET = _BV(pin_4[i]);
|
|
|
|
}
|
|
}
|
|
|
|
void init_port_expander()
|
|
{
|
|
I2C0_example_write1ByteRegister(32, 0x00, 0x00);
|
|
I2C0_example_write1ByteRegister(32, 0x09, 0x00);
|
|
}
|
|
|
|
void set_expanded_port_on(uint8_t num)
|
|
{
|
|
if (LISTEN_EXPANDER_COMMANDS) {
|
|
port_val = I2C0_example_read1ByteRegister(32, 0x09);
|
|
port_val |= _BV(num);
|
|
I2C0_example_write1ByteRegister(32, 0x09, port_val);
|
|
}
|
|
}
|
|
|
|
void set_expanded_port_off(uint8_t num)
|
|
{
|
|
if (LISTEN_EXPANDER_COMMANDS) {
|
|
port_val = I2C0_example_read1ByteRegister(32, 0x09);
|
|
port_val &= ~_BV(num);
|
|
I2C0_example_write1ByteRegister(32, 0x09, port_val);
|
|
}
|
|
}
|
|
|
|
void battery_charging_on()
|
|
{
|
|
set_expanded_port_on(0);
|
|
}
|
|
|
|
void battery_charging_off()
|
|
{
|
|
set_expanded_port_off(0);
|
|
}
|
|
|
|
void battery_power_supply_on()
|
|
{
|
|
set_expanded_port_on(1);
|
|
}
|
|
|
|
void battery_power_supply_off()
|
|
{
|
|
set_expanded_port_off(1);
|
|
}
|
|
|
|
void power_3v3_on()
|
|
{
|
|
// Turn on 3.3V relay
|
|
PORTA.DIRSET = _BV(6);
|
|
PORTA.OUTSET = _BV(6);
|
|
}
|
|
|
|
void power_3v3_off()
|
|
{
|
|
// Turn off 3.3V relay
|
|
PORTA.DIRSET = _BV(6);
|
|
PORTA.OUTCLR = _BV(6);
|
|
}
|
|
|
|
void delay_n_units(uint8_t n)
|
|
{
|
|
for (int i = 0; i < n; i++) {
|
|
_delay_us(delay_time);
|
|
}
|
|
|
|
};
|
|
|
|
void step_phased(struct PORT_struct * p1, struct PORT_struct * p2, struct PORT_struct * p3, struct PORT_struct * p4, uint8_t m1, uint8_t m2, uint8_t m3, uint8_t m4, uint8_t phase)
|
|
{
|
|
|
|
|
|
if (phase == 0) {
|
|
p1->OUTSET = _BV(m1);
|
|
p2->OUTCLR = _BV(m2);
|
|
p3->OUTSET = _BV(m3);
|
|
p4->OUTCLR = _BV(m4);
|
|
} else if (phase == 1) {
|
|
p1->OUTCLR = _BV(m1);
|
|
p2->OUTSET = _BV(m2);
|
|
p3->OUTSET = _BV(m3);
|
|
p4->OUTCLR = _BV(m4);
|
|
} else if (phase == 2) {
|
|
p1->OUTCLR = _BV(m1);
|
|
p2->OUTSET = _BV(m2);
|
|
p3->OUTCLR = _BV(m3);
|
|
p4->OUTSET = _BV(m4);
|
|
} else if (phase == 3) {
|
|
p1->OUTSET = _BV(m1);
|
|
p2->OUTCLR = _BV(m2);
|
|
p3->OUTCLR = _BV(m3);
|
|
p4->OUTSET = _BV(m4);
|
|
}
|
|
|
|
}
|
|
|
|
void step_motor(uint8_t i, int s)
|
|
{
|
|
|
|
|
|
for (int n = 0; n < abs(s); n++) {
|
|
|
|
phase = abs(current_step[i] % 4);
|
|
motor_max_pos = (current_step[i] >= max_steps[i]) && (s > 0);
|
|
motor_min_pos = (current_step[i] <= 0) && (s < 0);
|
|
|
|
if (motor_max_pos || motor_min_pos) {
|
|
if (!IGNORE_SOFT_LIMITS) {
|
|
return;
|
|
}
|
|
}
|
|
|
|
step_phased(port_1[i], port_2[i], port_3[i], port_4[i], pin_1[i], pin_2[i], pin_3[i], pin_4[i], phase);
|
|
delay_n_units(delay[i]);
|
|
|
|
if (s > 0) {
|
|
current_step[i]++;
|
|
} else if (s < 0) {
|
|
current_step[i]--;
|
|
}
|
|
}
|
|
|
|
|
|
}
|
|
|
|
void step_iris_phased(int steps)
|
|
{
|
|
set_expanded_port_on(4);
|
|
step_motor(2, steps);
|
|
set_expanded_port_off(4);
|
|
}
|
|
|
|
void step_focus_phased(int steps)
|
|
{
|
|
set_expanded_port_on(3);
|
|
step_motor(1, steps);
|
|
set_expanded_port_off(3);
|
|
}
|
|
|
|
void step_zoom_phased(int steps)
|
|
{
|
|
set_expanded_port_on(2);
|
|
step_motor(0, steps);
|
|
set_expanded_port_off(2);
|
|
}
|
|
|
|
bool at_zoom_limit()
|
|
{
|
|
if (PORTD.IN & PIN1_bm) {
|
|
return false;
|
|
} else {
|
|
return true;
|
|
}
|
|
}
|
|
|
|
bool at_focus_limit()
|
|
{
|
|
|
|
if (PORTD.IN & PIN0_bm) {
|
|
return false;
|
|
} else {
|
|
return true;
|
|
}
|
|
|
|
}
|
|
|
|
void home_zoom()
|
|
{
|
|
|
|
motor_num = 0;
|
|
set_expanded_port_on(motor_num + 2);
|
|
current_step[motor_num] = max_steps[motor_num] + 200;
|
|
step_motor(motor_num, -100);
|
|
_delay_ms(100);
|
|
for (int i = 0; i < max_steps[motor_num]; i++) {
|
|
step_motor(motor_num, -1);
|
|
if (at_zoom_limit()) {
|
|
break;
|
|
}
|
|
}
|
|
current_step[motor_num] = 0;
|
|
set_expanded_port_off(motor_num + 2);
|
|
}
|
|
|
|
void home_focus()
|
|
{
|
|
motor_num = 1;
|
|
set_expanded_port_on(motor_num + 2);
|
|
current_step[motor_num] = max_steps[motor_num] + 200;
|
|
step_motor(motor_num, -100);
|
|
_delay_ms(100);
|
|
for (int i = 0; i < max_steps[motor_num]; i++) {
|
|
step_motor(motor_num, -1);
|
|
if (at_focus_limit()) {
|
|
break;
|
|
}
|
|
}
|
|
current_step[motor_num] = 0;
|
|
set_expanded_port_off(motor_num + 2);
|
|
}
|
|
|
|
void home_iris()
|
|
{
|
|
motor_num = 2;
|
|
set_expanded_port_on(motor_num + 2);
|
|
current_step[motor_num] = max_steps[motor_num] + 200;
|
|
step_motor(motor_num, -100);
|
|
current_step[motor_num] = 0;
|
|
set_expanded_port_off(motor_num + 2);
|
|
}
|
|
|
|
void ir_filter_on()
|
|
{
|
|
PORTB.OUTCLR = _BV(2);
|
|
PORTB.OUTSET = _BV(3);
|
|
_delay_ms(100);
|
|
PORTB.OUTCLR = _BV(3);
|
|
}
|
|
|
|
void ir_filter_off()
|
|
{
|
|
PORTB.OUTCLR = _BV(3);
|
|
PORTB.OUTSET = _BV(2);
|
|
_delay_ms(100);
|
|
PORTB.OUTCLR = _BV(2);
|
|
}
|
|
|
|
void init_pins()
|
|
{
|
|
// Pins for reading endstops on lens assembly
|
|
PORTD.DIRCLR = PIN0_bm;
|
|
PORTD.DIRCLR = PIN1_bm;
|
|
|
|
PORTD.PIN0CTRL |= PORT_ISC_BOTHEDGES_gc;
|
|
PORTD.PIN1CTRL |= PORT_ISC_BOTHEDGES_gc;
|
|
|
|
// IR Filter
|
|
PORTB.DIRSET = _BV(2);
|
|
PORTB.DIRSET = _BV(3);
|
|
|
|
//i2c muxing
|
|
PORTMUX.TWISPIROUTEA = 0x23;
|
|
set_ports_out();
|
|
|
|
}
|
|
|
|
int main(void)
|
|
{
|
|
init_pins();
|
|
|
|
USART0_init();
|
|
I2C0_Initialize();
|
|
power_3v3_on();
|
|
init_port_expander();
|
|
sei();
|
|
if (!LISTEN_EXPANDER_COMMANDS) {
|
|
I2C0_example_write1ByteRegister(32, 0x09, 0xFF);
|
|
}
|
|
|
|
home_zoom();
|
|
home_focus();
|
|
home_iris();
|
|
while (1) {
|
|
|
|
|
|
}
|
|
return 0;
|
|
}
|