Last active
December 11, 2015 00:28
-
-
Save balrog-kun/4516478 to your computer and use it in GitHub Desktop.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
#include <avr/io.h> | |
#include <avr/interrupt.h> | |
#include "adc.h" | |
#include "timer1.h" | |
#include "uart.h" | |
#include "actuators.h" | |
static uint8_t motor[4] = { 0, 0, 0, 0 }; | |
static void show_state(void) { | |
serial_write_dec8(motor[0]); | |
serial_write_dec8(motor[1]); | |
serial_write_dec8(motor[2]); | |
serial_write_dec8(motor[3]); | |
serial_write_eol(); | |
} | |
static void motor_serial_init(void) { | |
PORTD &= ~(1 << 2); | |
DDRD |= 1 << 2; | |
} | |
static void motor_serial_tx(uint8_t val) { | |
#define pulse \ | |
"sbi %[PORT], 2\n\t" \ | |
"nop\n\t" \ | |
"nop\n\t" \ | |
"nop\n\t" \ | |
"nop\n\t" \ | |
"nop\n\t" \ | |
"nop\n\t" \ | |
"nop\n\t" \ | |
"nop\n\t" \ | |
"nop\n\t" \ | |
"nop\n\t" \ | |
"nop\n\t" \ | |
"nop\n\t" \ | |
"nop\n\t" \ | |
"nop\n\t" \ | |
"nop\n\t" \ | |
"nop\n\t" \ | |
"nop\n\t" \ | |
"nop\n\t" \ | |
"cbi %[PORT], 2\n\t" \ | |
"nop\n\t" \ | |
"nop\n\t" \ | |
"nop\n\t" \ | |
"nop\n\t" \ | |
"nop\n\t" \ | |
"nop\n\t" \ | |
"nop\n\t" \ | |
"nop\n\t" \ | |
"nop\n\t" \ | |
"nop\n\t" \ | |
"nop\n\t" \ | |
"nop\n\t" \ | |
"nop\n\t" \ | |
"nop\n\t" \ | |
"nop\n\t" \ | |
"nop\n\t" \ | |
"nop\n\n" \ | |
"nop\n\t" \ | |
"nop\n\t" \ | |
"nop\n\t" \ | |
"nop \n\t" \ | |
"nop\n\t" \ | |
"nop\n\t" \ | |
"nop\n\t" \ | |
"nop\n\t" \ | |
"nop\n\t" \ | |
"nop\n\t" \ | |
"nop\n\t" \ | |
"nop\n\t" \ | |
"nop\n\t" \ | |
"nop\n\t" \ | |
"nop\n\t" \ | |
"nop\n\t" \ | |
"nop\n\t" \ | |
"nop\n\t" \ | |
"nop\n\t" \ | |
"nop\n\t" \ | |
"nop\n\t" \ | |
"nop\n\t" \ | |
"nop\n\t" \ | |
"nop\n\t" \ | |
"nop\n\t" \ | |
"nop\n\t" \ | |
" nop\n\t" \ | |
"nop\n\t" \ | |
"nop\n\t" \ | |
"nop\n\t" \ | |
"nop\n\t" \ | |
"nop\n\t" \ | |
"nop\n\t" \ | |
"nop\n\t" \ | |
"nop\n\t" \ | |
"nop\n\t" \ | |
"nop\n\t" \ | |
"nop\n\t" \ | |
"nop\n\t" \ | |
"nop\n\t" \ | |
"nop\n\t" \ | |
"nop\n\t" \ | |
"nop\n\t" \ | |
"nop\n\t" \ | |
"nop\n\t" \ | |
"nop\n\t" \ | |
"nop\n\t" \ | |
"nop\n\t" \ | |
"nop\n\t" \ | |
"nop\n\t" \ | |
"nop\n\t" \ | |
"nop\n\t" \ | |
"nop\n\t" \ | |
"nop\n\t" \ | |
"nop\n\t" \ | |
"nop\n\t" \ | |
"nop\n\t" \ | |
"nop\n\t" \ | |
"nop\n\t" \ | |
"nop\n\t" \ | |
"nop\n\t" \ | |
"nop\n\t" \ | |
"nop\n\t" \ | |
"nop\n\t" \ | |
"nop\n\t" | |
asm volatile ( | |
"cli\n\t" | |
/* Set ESC's MOSI direction */ | |
"sbi %[DDR], 3\n\t" | |
/* This is a write operation */ | |
"sbi %[PORT], 3\n\t" | |
pulse | |
"sbrc %[val], 7\n\t" | |
"sbi %[PORT], 3\n\t" | |
"sbrs %[val], 7\n\t" | |
"cbi %[PORT], 3\n\t" | |
pulse | |
"sbrc %[val], 6\n\t" | |
"sbi %[PORT], 3\n\t" | |
"sbrs %[val], 6\n\t" | |
"cbi %[PORT], 3\n\t" | |
pulse | |
"sbrc %[val], 5\n\t" | |
"sbi %[PORT], 3\n\t" | |
"sbrs %[val], 5\n\t" | |
"cbi %[PORT], 3\n\t" | |
pulse | |
"sbrc %[val], 4\n\t" | |
"sbi %[PORT], 3\n\t" | |
"sbrs %[val], 4\n\t" | |
"cbi %[PORT], 3\n\t" | |
pulse | |
"sbrc %[val], 3\n\t" | |
"sbi %[PORT], 3\n\t" | |
"sbrs %[val], 3\n\t" | |
"cbi %[PORT], 3\n\t" | |
pulse | |
"sbrc %[val], 2\n\t" | |
"sbi %[PORT], 3\n\t" | |
"sbrs %[val], 2\n\t" | |
"cbi %[PORT], 3\n\t" | |
pulse | |
"sbrc %[val], 1\n\t" | |
"sbi %[PORT], 3\n\t" | |
"sbrs %[val], 1\n\t" | |
"cbi %[PORT], 3\n\t" | |
pulse | |
"sbrc %[val], 0\n\t" | |
"sbi %[PORT], 3\n\t" | |
"sbrs %[val], 0\n\t" | |
"cbi %[PORT], 3\n\t" | |
pulse | |
"sei\n\t" | |
:: [val] "r" (val), | |
[PIN] "i" ((uint16_t) 0x09), | |
[DDR] "i" ((uint16_t) 0x0a), | |
[PORT] "i" ((uint16_t) 0x0b)); | |
} | |
static uint8_t motor_serial_rx(uint8_t byte) { | |
register uint8_t j; | |
asm volatile ( | |
"cli\n\t" | |
/* Set ESC's MOSI direction */ | |
"sbi %[DDR], 3\n\t" | |
/* This is a read operation */ | |
"cbi %[PORT], 3\n\t" | |
pulse | |
/* We would like the [addr]th byte, please */ | |
"sbrc %[addr], 0\n\t" | |
"sbi %[PORT], 3\n\t" | |
pulse | |
/* Reset ESC's MOSI pull-up & direction */ | |
"cbi %[PORT], 3\n\t" | |
"cbi %[DDR], 3\n\t" | |
"clr %[val]\n\t" | |
"nop\n\t" | |
"nop\n\t" | |
"nop\n\t" | |
"nop\n\t" | |
"sbic %[PIN], 3\n\t" | |
"sbr %[val], 1 << 7\n\t" | |
pulse | |
"sbic %[PIN], 3\n\t" | |
"sbr %[val], 1 << 6\n\t" | |
pulse | |
"sbic %[PIN], 3\n\t" | |
"sbr %[val], 1 << 5\n\t" | |
pulse | |
"sbic %[PIN], 3\n\t" | |
"sbr %[val], 1 << 4\n\t" | |
pulse | |
"sbic %[PIN], 3\n\t" | |
"sbr %[val], 1 << 3\n\t" | |
pulse | |
"sbic %[PIN], 3\n\t" | |
"sbr %[val], 1 << 2\n\t" | |
pulse | |
"sbic %[PIN], 3\n\t" | |
"sbr %[val], 1 << 1\n\t" | |
pulse | |
"sbic %[PIN], 3\n\t" | |
"sbr %[val], 1 << 0\n\t" | |
"sei\n\t" | |
: [val] "=r" (j) | |
: [addr] "r" (byte), | |
[PIN] "i" ((uint16_t) 0x09), | |
[DDR] "i" ((uint16_t) 0x0a), | |
[PORT] "i" ((uint16_t) 0x0b)); | |
return j; | |
} | |
static void handle_input(char ch) { | |
uint16_t com_count; | |
static uint32_t prev_ts = 0; | |
uint32_t ts; | |
switch (ch) { | |
#define MOTOR_DOWN(n) \ | |
if (motor[n] > 0) \ | |
motor[n] --; \ | |
actuator_set(n, motor[n] << 8); \ | |
break; | |
#define MOTOR_UP(n) \ | |
if (motor[n] < 255) \ | |
motor[n] ++; \ | |
actuator_set(n, motor[n] << 8); \ | |
break; | |
case 'a': | |
MOTOR_DOWN(0); | |
case 's': | |
MOTOR_DOWN(1); | |
case 'd': | |
MOTOR_DOWN(2); | |
case 'f': | |
MOTOR_DOWN(3); | |
case 'q': | |
MOTOR_UP(0); | |
case 'w': | |
MOTOR_UP(1); | |
case 'e': | |
MOTOR_UP(2); | |
case 'r': | |
MOTOR_UP(3); | |
default: | |
return; | |
} | |
sei(); /* We can re-enable interrupts already */ | |
show_state(); | |
/* Read motor commutation step count */ | |
ts = timer_read(); | |
com_count = motor_serial_rx(0); | |
my_delay(1); | |
com_count |= (uint16_t) motor_serial_rx(1) << 8; | |
my_delay(1); | |
motor_serial_tx(motor[0]); | |
com_count -= 0x0505; | |
ts -= prev_ts; | |
prev_ts += ts; | |
/* #electrical revs = #commutation steps / 6 */ | |
/* Elecrtival revolutions / s = #electrical revs / time-period */ | |
/* Mechanical revolutions / s = Electrical revolutions / s / #poles * 2 */ | |
#define POLES 14 | |
serial_write_fp32((uint32_t) com_count * (F_CPU * 2 / POLES / 64 / 6), | |
ts >> 6); | |
serial_write_str(" RPS\r\n"); | |
} | |
void setup(void) { | |
uint8_t s = SREG; | |
uint8_t m = MCUCR; | |
serial_init(); | |
adc_init(); | |
timer_init(); | |
sei(); | |
//actuators_init(4); | |
motor_serial_init(); | |
serial_set_handler(handle_input); | |
/* Wait for someone to attach to UART */ | |
my_delay(4000); | |
serial_write_str("SREG:"); | |
serial_write_hex16(s); | |
serial_write_str(", MCUCR:"); | |
serial_write_hex16(m); | |
serial_write_eol(); | |
/* Wait for the ESCs to detect voltages etc. */ | |
my_delay(4000); | |
//actuators_start(); | |
show_state(); | |
} | |
void loop(void) { | |
my_delay(10); | |
/*serial_write1('.');*/ | |
/*motor_serial_txrx(motor[0]);*/ | |
} | |
int main(void) { | |
setup(); | |
for (;;) | |
loop(); | |
return 0; | |
} |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment