thebears/birdptz
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

yacwc

Browse Source
This commit is contained in:
Ishan S. Patel
2021-09-17 15:13:02 -04:00
parent cb75206770
commit 83ac9e2979
14 changed files with 468 additions and 426 deletions
Download Patch File Download Diff File Expand all files Collapse all files

608
main.c
View File

@@ -1,10 +1,3 @@
/*
* File: main.c
* Author: thebears
*
* Created on September 13, 2021, 3:12 PM
*/
#define F_CPU 3333333
#define USART0_BAUD_RATE(BAUD_RATE) ((float)(3333333 * 64 / (16 * (float)BAUD_RATE)) + 0.5)
#include <avr/io.h>
@@ -14,419 +7,278 @@
#include <stdlib.h>
#include <stdio.h>
#include "helpers_uart.h"
//#include "helpers_i2c.h"
#include <avr/interrupt.h>
#include <xc.h>
#include "twi0_master_example.h"
uint16_t adcVal;
uint8_t val;
void ADC0_init(void);
uint16_t ADC0_read_a(void);
uint16_t ADC0_read_b(void);
bool limit_zoom = false;
bool limit_focus = false;
uint16_t step_n_zoom = 0;
uint16_t step_n_focus = 0;
uint16_t step_n_iris = 0;
#define delay_time 50
// Zoom, Focus, Iris
struct PORT_struct *port_1[] = {&PORTA, &PORTB, &PORTC};
struct PORT_struct *port_2[] = {&PORTA, &PORTB, &PORTC};
struct PORT_struct *port_3[] = {&PORTA, &PORTC, &PORTC};
struct PORT_struct *port_4[] = {&PORTA, &PORTC, &PORTC};
#define max_zoom_steps 2994
#define max_focus_steps 5180
#define max_iris_steps 77
uint8_t pin_1[] = {2, 5, 4};
uint8_t pin_2[] = {3, 4, 5};
uint8_t pin_3[] = {4, 1, 6};
uint8_t pin_4[] = {5, 0, 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;
int8_t last_dir_zoom = 0;
int8_t last_dir_focus = 0;
uint8_t motor_num;
uint8_t port_val;
bool focus_iris_max;
bool focus_iris_min;
bool focus_zoom_max;
bool focus_zoom_min;
bool focus_past_max;
bool focus_past_min;
// Zoom
#define m1_p1 2
#define m1_p2 3
#define m1_p3 4
#define m1_p4 5
#define deltime1 750
void init_port_expander()
{
I2C0_example_write1ByteRegister(32, 0x00, 0x00);
I2C0_example_write1ByteRegister(32, 0x00, 0x00);
}
// Focus
#define m2_p1 5
#define m2_p2 4
#define m2_p3 1
#define m2_p4 0
#define deltime2 750
void set_expanded_port_on(uint8_t num)
{
port_val = I2C0_example_read1ByteRegister(32, 0x09);
port_val |= _BV(num);
I2C0_example_write1ByteRegister(32, 0x09, port_val);
}
// P-Iris
#define m3_p1 4
#define m3_p2 5
#define m3_p3 6
#define m3_p4 7
#define deltime3 2500
#define delay_time 100
void set_expanded_port_off(uint8_t num)
{
port_val = I2C0_example_read1ByteRegister(32, 0x09);
port_val &= ~_BV(num);
I2C0_example_write1ByteRegister(32, 0x09, port_val);
}
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) {
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);
}
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 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_iris_phased(int steps) {
void step_motor(uint8_t i, int s)
{
set_expanded_port_on(i);
for (int n = 0; i < 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);
for (int i = 0; i < abs(steps); i++) {
phase = abs(step_n_iris % 4);
focus_iris_max = (step_n_iris >= max_iris_steps) && (steps > 0);
focus_iris_min = (step_n_iris <= 0) && (steps < 0);
if (motor_max_pos || motor_min_pos) {
return;
}
if (focus_iris_max || focus_iris_min) {
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]);
step_phased(&PORTC, &PORTC, &PORTC, &PORTC, m3_p1, m3_p2, m3_p3, m3_p4, phase);
_delay_us(deltime3);
if (steps > 0) {
step_n_iris++;
} else if (steps < 0) {
step_n_iris--;
}
}
}
if (s > 0) {
current_step[i]++;
} else if (s < 0) {
current_step[i]--;
}
}
void step_zoom_phased(int steps) {
for (int i = 0; i < abs(steps); i++) {
phase = abs(step_n_zoom % 4);
focus_zoom_max = (step_n_zoom >= max_zoom_steps) && (steps > 0);
focus_zoom_min = (step_n_zoom <= 0) && (steps < 0);
if (focus_zoom_max || focus_zoom_min) {
return;
}
step_phased(&PORTA, &PORTA, &PORTA, &PORTA, m1_p1, m1_p2, m1_p3, m1_p4, phase);
_delay_us(deltime1);
if (steps > 0) {
step_n_zoom++;
} else if (steps < 0) {
step_n_zoom--;
}
}
}
void step_focus_phased(int steps) {
for (int i = 0; i < abs(steps); i++) {
focus_past_max = (step_n_focus >= max_focus_steps) && (steps > 0);
focus_past_min = (step_n_focus <= 0) && (steps < 0);
if (focus_past_max || focus_past_min) {
return;
}
void step_iris_phased(int steps)
{
set_expanded_port_on(2);
step_motor(2, steps);
set_expanded_port_off(2);
}
phase = abs(step_n_focus % 4);
step_phased(&PORTB, &PORTB, &PORTC, &PORTC, m2_p1, m2_p2, m2_p3, m2_p4, phase);
_delay_us(deltime2);
if (steps > 0) {
step_n_focus++;
} else if (steps < 0) {
step_n_focus--;
}
}
void step_zoom_phased(int steps)
{
set_expanded_port_on(1);
step_motor(1, steps);
set_expanded_port_off(1);
}
void step_focus_phased(int steps)
{
set_expanded_port_on(0);
step_motor(0, steps);
set_expanded_port_off(0);
}
bool at_zoom_limit()
{
if (PORTD.IN & PIN0_bm) {
return false;
} else {
return true;
}
}
bool at_focus_limit()
{
if (PORTD.IN & PIN1_bm) {
return false;
} else {
return true;
}
}
bool update_zoom_limit() {
if (PORTD.IN & PIN0_bm) {
limit_zoom = false;
} else {
limit_zoom = true;
}
return limit_zoom;
void home_focus()
{
motor_num = 0;
current_step[motor_num] = max_steps[motor_num] + 200;
step_motor(motor_num, -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;
}
bool update_focus_limit() {
if (PORTD.IN & PIN1_bm) {
limit_focus = false;
} else {
limit_focus = true;
}
return limit_focus;
void home_zoom()
{
motor_num = 1;
current_step[motor_num] = max_steps[motor_num] + 200;
step_motor(motor_num, -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;
}
void ir_filter_on() {
PORTB.OUTCLR = _BV(2);
PORTB.OUTSET = _BV(3);
_delay_ms(100);
PORTB.OUTCLR = _BV(3);
void home_iris()
{
motor_num = 3;
current_step[motor_num] = max_steps[motor_num] + 200;
step_motor(motor_num, -100);
current_step[motor_num] = 0;
}
void ir_filter_off() {
PORTB.OUTCLR = _BV(3);
PORTB.OUTSET = _BV(2);
_delay_ms(100);
PORTB.OUTCLR = _BV(2);
void ir_filter_on()
{
PORTB.OUTCLR = _BV(2);
PORTB.OUTSET = _BV(3);
_delay_ms(100);
PORTB.OUTCLR = _BV(3);
}
void home_focus() {
step_n_focus = max_focus_steps + 200;
step_focus_phased(-100);
for (int i = 0; i < max_focus_steps; i++) {
step_focus_phased(-1);
if (update_focus_limit()) {
break;
}
}
step_n_focus = 0;
void ir_filter_off()
{
PORTB.OUTCLR = _BV(3);
PORTB.OUTSET = _BV(2);
_delay_ms(100);
PORTB.OUTCLR = _BV(2);
}
void home_zoom() {
step_n_zoom = max_zoom_steps + 200;
step_zoom_phased(-100);
for (int i = 0; i < max_zoom_steps; i++) {
step_zoom_phased(-1);
if (update_zoom_limit()) {
break;
}
}
step_n_zoom = 0;
void init_pins()
{
// Pins for reading endstops on lens assembly
PORTD.DIRCLR = PIN0_bm;
PORTD.DIRCLR = PIN1_bm;
// IR Filter
PORTB.DIRSET = _BV(2);
PORTB.DIRSET = _BV(3);
//i2c muxing
PORTMUX.TWISPIROUTEA = 0x23;
}
void home_iris() {
step_n_iris = max_iris_steps + 200;
step_iris_phased(-100);
step_n_iris = 0;
int main(void)
{
I2C0_Initialize();
init_port_expander();
USART0_init();
while (1) {
step_zoom_phased(6000);
step_iris_phased(100);
step_focus_phased(2000);
step_zoom_phased(-6000);
step_iris_phased(-100);
step_focus_phased(-2000);
}
return 0;
}
ISR(PORTD_PORT_vect) {
if (PORTD.INTFLAGS & PIN1_bm) {
update_focus_limit();
PORTD.INTFLAGS &= PIN1_bm;
} else if (PORTD.INTFLAGS & PIN0_bm) {
update_zoom_limit();
PORTD.INTFLAGS &= PIN0_bm;
};
}
int main(void) {
PORTA.OUTSET = _BV(6);
PORTD.DIRCLR = PIN0_bm;
PORTD.DIRCLR = PIN1_bm;
PORTD.PIN0CTRL |= PORT_ISC_BOTHEDGES_gc;
PORTD.PIN1CTRL |= PORT_ISC_BOTHEDGES_gc;
// Turn on 3.3V relay
PORTA.DIRSET = _BV(6);
// Zoom
PORTA.DIRSET = _BV(m1_p1);
PORTA.DIRSET = _BV(m1_p4);
PORTA.DIRSET = _BV(m1_p2);
PORTA.DIRSET = _BV(m1_p3);
// Focus
PORTB.DIRSET = _BV(m2_p1);
PORTB.DIRSET = _BV(m2_p2);
PORTC.DIRSET = _BV(m2_p3);
PORTC.DIRSET = _BV(m2_p4);
// Iris motor
PORTC.DIRSET = _BV(m3_p1);
PORTC.DIRSET = _BV(m3_p2);
PORTC.DIRSET = _BV(m3_p3);
PORTC.DIRSET = _BV(m3_p4);
// IR Filter
PORTB.DIRSET = _BV(2);
PORTB.DIRSET = _BV(3);
// sei();
PORTMUX.TWISPIROUTEA = 0x23;
_delay_ms(250);
I2C0_Initialize();
I2C0_example_write1ByteRegister(32, 0x00, 0x00);
I2C0_example_write1ByteRegister(32, 0x09, 0xFF);
USART0_init();
USART0_sendString("Started\n");
_delay_ms(250);
I2C0_example_write1ByteRegister(32, 0x09, 0x00);
_delay_ms(250);
I2C0_example_write1ByteRegister(32, 0x09, 0xFF);
USART0_sendString("focus->");
home_focus();
USART0_sendString("\niris->");
home_iris();
USART0_sendString("\nhome->");
home_zoom();
USART0_sendString("\n");
while (1) {
step_zoom_phased(6000);
step_iris_phased(100);
step_focus_phased(2000);
step_zoom_phased(-6000);
//
step_iris_phased(-100);
step_focus_phased(-2000);
_delay_ms(250);
// ir_filter_off();
// step_iris_phased(76);
// step_iris_phased(-76);
// _delay_ms(25);
// ir_filter_on();
// step_iris(19);
//_delay_ms(250);
// _delay_ms(500);
// _delay_ms(500);
//
// step_focus(1000);
// step_focus(-1000);
// limit_focus = false;
// step_focus(-1000);
// limit_focus = false;
// step_zoom(1000);
// limit_zoom = false;
// step_zoom(-1000);
// limit_zoom = false;
// step_focus(1000);
// if (limit_focus)
// {
// limit_focus = false;
// step_focus(-1000);
// }
// step(1000);
// step2(1000);
// _delay_ms(100);
USART0_sendString("-");
// uart_print_uint8(PORTD.IN & PIN0_bm,"p0 ");
// uart_print_uint8(PORTD.IN & PIN1_bm,"p1 \n ")
// step2(1000);
// PORTA.OUTSET = _BV(m1_p1);
// PORTA.OUTSET = _BV(m1_p2);
// PORTA.OUTSET = _BV(m1_p3);
// PORTA.OUTSET = _BV(m1_p4);
//
// PORTB.OUTSET = _BV(m2_p1);
// PORTB.OUTSET = _BV(m2_p2);
// PORTC.OUTSET = _BV(m2_p3);
// PORTC.OUTSET = _BV(m2_p4);
// uart_print_binary(val, " \n");
// adcVal = ADC0_read_a();
// uart_print_uint8(adcVal, " ");
// adcVal = ADC0_read_b();
// uart_print_uint8(adcVal, " ");
// step();
// step2();
// USART0_sendString("---\n");
}
return 0;
}
void ADC0_init(void) {
PORTD.PIN0CTRL &= ~PORT_ISC_gm;
PORTD.PIN0CTRL |= PORT_ISC_INPUT_DISABLE_gc;
PORTD.PIN0CTRL &= ~PORT_PULLUPEN_bm;
PORTD.PIN1CTRL &= ~PORT_ISC_gm;
PORTD.PIN1CTRL |= PORT_ISC_INPUT_DISABLE_gc;
PORTD.PIN1CTRL &= ~PORT_PULLUPEN_bm;
ADC0.CTRLC = ADC_PRESC_DIV4_gc
| ADC_REFSEL_VDDREF_gc;
ADC0.CTRLA = ADC_ENABLE_bm /* ADC Enable: enabled */
| ADC_RESSEL_10BIT_gc; /* 10-bit mode */
/* Select ADC channel */
}
uint16_t ADC0_read_a(void) {
/* Start ADC conversion */
/* Start ADC conversion */
ADC0.MUXPOS = ADC_MUXPOS_AIN0_gc;
ADC0.COMMAND = ADC_STCONV_bm;
/* Wait until ADC conversion done */
while (!(ADC0.INTFLAGS & ADC_RESRDY_bm)) {
;
}
/* Clear the interrupt flag by writing 1: */
ADC0.INTFLAGS = ADC_RESRDY_bm;
return ADC0.RES;
}
uint16_t ADC0_read_b(void) {
/* Start ADC conversion */
/* Start ADC conversion */
ADC0.MUXPOS = ADC_MUXPOS_AIN1_gc;
ADC0.COMMAND = ADC_STCONV_bm;
/* Wait until ADC conversion done */
while (!(ADC0.INTFLAGS & ADC_RESRDY_bm)) {
;
}
/* Clear the interrupt flag by writing 1: */
ADC0.INTFLAGS = ADC_RESRDY_bm;
return ADC0.RES;
}