Skip to content

Instantly share code, notes, and snippets.

@balrog-kun

balrog-kun/motor-test-serial.c

Last active December 11, 2015 00:28
Show Gist options
  • Save balrog-kun/4516478 to your computer and use it in GitHub Desktop.
Save balrog-kun/4516478 to your computer and use it in GitHub Desktop.
Download ZIP
Raw
motor-test-serial.c
#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