From 4924631781383e598376b3d952a87ffefd37a444 Mon Sep 17 00:00:00 2001 From: AndReGeist Date: Fri, 3 Jun 2022 18:27:09 +0200 Subject: [PATCH] Added firmware used to run the robot --- firmware/M2-on-wheelbot/Makefile | 127 ++ firmware/M2-on-wheelbot/inc/checks.h | 46 + firmware/M2-on-wheelbot/inc/config.h | 116 ++ firmware/M2-on-wheelbot/inc/control.h | 37 + firmware/M2-on-wheelbot/inc/icm20948.h | 196 ++++ firmware/M2-on-wheelbot/inc/m_bus.h | 59 + firmware/M2-on-wheelbot/inc/m_crc32.h | 9 + firmware/M2-on-wheelbot/inc/m_general.h | 100 ++ firmware/M2-on-wheelbot/inc/m_rf.h | 62 + firmware/M2-on-wheelbot/inc/m_spi.h | 409 +++++++ firmware/M2-on-wheelbot/inc/m_usb.h | 189 +++ .../M2-on-wheelbot/inc/matrix_calculations.h | 14 + .../M2-on-wheelbot/inc/state_estimation.h | 42 + firmware/M2-on-wheelbot/inc/udriver.h | 93 ++ firmware/M2-on-wheelbot/src/checks.c | 100 ++ firmware/M2-on-wheelbot/src/control.c | 103 ++ firmware/M2-on-wheelbot/src/icm20948.c | 88 ++ firmware/M2-on-wheelbot/src/m_bus.c | 116 ++ firmware/M2-on-wheelbot/src/m_crc32.c | 175 +++ firmware/M2-on-wheelbot/src/m_rf.c | 153 +++ firmware/M2-on-wheelbot/src/m_spi.c | 198 ++++ firmware/M2-on-wheelbot/src/m_usb.c | 1038 +++++++++++++++++ firmware/M2-on-wheelbot/src/main.c | 946 +++++++++++++++ .../M2-on-wheelbot/src/matrix_calculations.c | 45 + .../M2-on-wheelbot/src/state_estimation.c | 134 +++ firmware/M2-on-wheelbot/src/udriver.c | 262 +++++ firmware/M2-wifi-dongle/Makefile | 66 ++ firmware/M2-wifi-dongle/m_bus.c | 116 ++ firmware/M2-wifi-dongle/m_bus.h | 59 + firmware/M2-wifi-dongle/m_crc32.c | 181 +++ firmware/M2-wifi-dongle/m_crc32.h | 10 + firmware/M2-wifi-dongle/m_general.h | 114 ++ firmware/M2-wifi-dongle/m_rf.c | 153 +++ firmware/M2-wifi-dongle/m_rf.h | 62 + firmware/M2-wifi-dongle/m_usb.c | 1023 ++++++++++++++++ firmware/M2-wifi-dongle/m_usb.h | 186 +++ firmware/M2-wifi-dongle/main.c | 129 ++ .../M2_analyzeData.py | 307 +++++ .../M2_pythonSerial.py | 421 +++++++ firmware/computer-python-interface/Pipfile | 17 + 40 files changed, 7701 insertions(+) create mode 100644 firmware/M2-on-wheelbot/Makefile create mode 100644 firmware/M2-on-wheelbot/inc/checks.h create mode 100644 firmware/M2-on-wheelbot/inc/config.h create mode 100644 firmware/M2-on-wheelbot/inc/control.h create mode 100644 firmware/M2-on-wheelbot/inc/icm20948.h create mode 100644 firmware/M2-on-wheelbot/inc/m_bus.h create mode 100644 firmware/M2-on-wheelbot/inc/m_crc32.h create mode 100644 firmware/M2-on-wheelbot/inc/m_general.h create mode 100644 firmware/M2-on-wheelbot/inc/m_rf.h create mode 100644 firmware/M2-on-wheelbot/inc/m_spi.h create mode 100644 firmware/M2-on-wheelbot/inc/m_usb.h create mode 100644 firmware/M2-on-wheelbot/inc/matrix_calculations.h create mode 100644 firmware/M2-on-wheelbot/inc/state_estimation.h create mode 100644 firmware/M2-on-wheelbot/inc/udriver.h create mode 100644 firmware/M2-on-wheelbot/src/checks.c create mode 100644 firmware/M2-on-wheelbot/src/control.c create mode 100644 firmware/M2-on-wheelbot/src/icm20948.c create mode 100644 firmware/M2-on-wheelbot/src/m_bus.c create mode 100644 firmware/M2-on-wheelbot/src/m_crc32.c create mode 100644 firmware/M2-on-wheelbot/src/m_rf.c create mode 100644 firmware/M2-on-wheelbot/src/m_spi.c create mode 100644 firmware/M2-on-wheelbot/src/m_usb.c create mode 100644 firmware/M2-on-wheelbot/src/main.c create mode 100644 firmware/M2-on-wheelbot/src/matrix_calculations.c create mode 100644 firmware/M2-on-wheelbot/src/state_estimation.c create mode 100644 firmware/M2-on-wheelbot/src/udriver.c create mode 100644 firmware/M2-wifi-dongle/Makefile create mode 100644 firmware/M2-wifi-dongle/m_bus.c create mode 100644 firmware/M2-wifi-dongle/m_bus.h create mode 100644 firmware/M2-wifi-dongle/m_crc32.c create mode 100644 firmware/M2-wifi-dongle/m_crc32.h create mode 100644 firmware/M2-wifi-dongle/m_general.h create mode 100644 firmware/M2-wifi-dongle/m_rf.c create mode 100644 firmware/M2-wifi-dongle/m_rf.h create mode 100644 firmware/M2-wifi-dongle/m_usb.c create mode 100644 firmware/M2-wifi-dongle/m_usb.h create mode 100644 firmware/M2-wifi-dongle/main.c create mode 100644 firmware/computer-python-interface/M2_analyzeData.py create mode 100644 firmware/computer-python-interface/M2_pythonSerial.py create mode 100644 firmware/computer-python-interface/Pipfile diff --git a/firmware/M2-on-wheelbot/Makefile b/firmware/M2-on-wheelbot/Makefile new file mode 100644 index 0000000..e52e8f7 --- /dev/null +++ b/firmware/M2-on-wheelbot/Makefile @@ -0,0 +1,127 @@ +# -------------------------------------------------------- +# Custom ATmega Makefile +# created by: Chao Liu(chaoliu@seas.upenn.edu) +# updated: Aug 28, 2016 +# -------------------------------------------------------- + +# -------------------------------------------------------- +# Support atmega88a, atmega168a and atmega32u4 +# -------------------------------------------------------- + +# -------------------------------------------------------- +# you shouldn't change anything below here, +# unless you really know what you're doing +# -------------------------------------------------------- + +# -------------------------------------------------------- +# Specify the device you are using and its clock. +# -------------------------------------------------------- + +DEVICE = atmega32u4 +CLOCK = 16000000 + +# -------------------------------------------------------- +# if you are using JTAGICE mkII, let PROGRAMMER = jtag2isp; +# if you are using AVRISP mkII, let PROGRAMMER = avrispmkII; +# if you are using USB, let PROGRAMMER = USB. +# -------------------------------------------------------- + +PROGRAMMER = USB + +ifeq ($(DEVICE), atmega88a) + TARGET_DEVICE = m88 + DEVICE_LABEL = ATmega88a +else ifeq ($(DEVICE), atmega168a) + TARGET_DEVICE = m168 + DEVICE_LABEL = ATmega168a +else ifeq ($(DEVICE), atmega32u4) + TARGET_DEVICE = m32u4 + DEVICE_LABEL = ATmega32U4 +else +$(error DEVICE = $(DEVICE) is unknown.) +endif + +COMPILE = avr-gcc -Wall -Os -DF_CPU=$(CLOCK) -mmcu=$(DEVICE) + +SRCDIR = src +INCDIR = inc # directory for header files + +INCLUDES += -I$(INCDIR) + +_SOURCES += $(wildcard $(SRCDIR)/*.c) +SOURCES = $(notdir $(_SOURCES)) + +OBJDIRS = obj_$(DEVICE) +OBJECTS := $(patsubst %.c,%.o, $(SOURCES)) +OBJECTS_POS = $(addprefix $(OBJDIRS)/,$(OBJECTS)) + +vpath %.c $(dir $(_SOURCES)) # directory for source files +vpath %.o $(OBJDIRS) # directory for object files +vpath %.elf $(OBJDIRS) +vpath %.hex . + +# symbolic targets: +all: main.hex +.PHONY : all + +.c.o: + @[ ! -e $@ ] && mkdir -p $(OBJDIRS) + @$(COMPILE) $(INCLUDES) -c $< -o $(OBJDIRS)/$@ + @echo "[CC] $^" + +.S.o: + @$(COMPILE) $(INCLUDES) -x assembler-with-cpp -c $< -o $(OBJDIRS)/$@ + @echo "[>-----Generate $@ Successfully-----<]" + +.c.s: + @$(COMPILE) $(INCLUDES) -S $< -o $(OBJDIRS)/$@ + @echo "[>-----Generate $@ Successfully-----<]" + +fuse: +ifeq ($(DEVICE), $(filter $(DEVICE), atmega88a atmega168a)) + @avrdude -p $(TARGET_DEVICE) -c $(PROGRAMMER) -P usb -U lfuse:w:0xe2:m -U hfuse:w:0xdf:m -B10 + @echo "[>-----Program Fuse Done-----<]" +else ifeq ($(DEVICE), atmega32u4) + @avrdude -p $(TARGET_DEVICE) -c $(PROGRAMMER) -P usb -U lfuse:w:0xe2:m -U hfuse:w:0xdf:m -B10 + @echo "[>-----Program Fuse Done-----<]" +endif + +install: flash + +flash: all +ifeq ($(DEVICE), $(filter $(DEVICE), atmega88a atmega168a)) + @avrdude -p $(TARGET_DEVICE) -c $(PROGRAMMER) -P usb -e -U flash:w:main.hex -B9 + @echo "[>-----$(DEVICE_LABEL) Loaded-----<]" +endif +ifeq ($(DEVICE), atmega32u4) +ifneq ($(PROGRAMMER), USB) + @avrdude -p $(TARGET_DEVICE) -c $(PROGRAMMER) -P usb -e -U flash:w:main.hex -B9 + @echo "[>-----$(DEVICE_LABEL) Loaded-----<]" +else + @dfu-programmer $(DEVICE) erase + @dfu-programmer $(DEVICE) flash main.hex + @echo "[>-----$(DEVICE_LABEL) Loaded-----<]" +endif +endif + +clean: + rm -fr main.hex $(OBJDIRS) + +# file targets: +main.elf: $(OBJECTS) + @$(COMPILE) -o $(OBJDIRS)/main.elf $(OBJECTS_POS) -lm + @echo "[ELF] $(OBJDIRS)/$@" + +main.hex: main.elf + @rm -f main.hex + @avr-objcopy -j .text -j .data -O ihex $(OBJDIRS)/main.elf main.hex + @avr-size --format=avr --mcu=$(DEVICE) $(OBJDIRS)/main.elf + @echo "[>-----Generate $@ Successfully-----<]" + @echo "[>-----Build Successfully-----<]" + +# Targets for code debugging and analysis: +disasm: main.elf + avr-objdump -d $(OBJDIRS)/main.elf + +cpp: + $(COMPILE) -E main.c diff --git a/firmware/M2-on-wheelbot/inc/checks.h b/firmware/M2-on-wheelbot/inc/checks.h new file mode 100644 index 0000000..963f6b6 --- /dev/null +++ b/firmware/M2-on-wheelbot/inc/checks.h @@ -0,0 +1,46 @@ +#ifndef MAEVARM_M2_CHECKS +#define MAEVARM_M2_CHECKS + +/* global program files */ +#include "config.h" +#include "m_general.h" +#include "udriver.h" + +typedef struct flags_struct { + bool system; // System safe? + bool wifi; // WiFi still connected? + bool angle; // Angle small during stabilization control? + bool rate; // Rate below MAX_RATE_MOTOR? + bool control; // Motor current below MAX_CURRENT_MOTOR? + bool battery; // Battery voltage larger than MIN_BATTERY_VOLTAGE? + bool upright; // Set when Wheelbot reaches upright position + bool standup1; // Set when standup1 started + bool standup1_done; // Set when standup1 done + bool standup2; // Set when standup2 started + bool standup2_done; // Set when standup2 done + bool bias; // Set when estimator bias computation started + bool loop_finished; // Set when MAIN WHILE loop has een fully executed + bool LQRcontrol1; // Set when roll control starts + bool LQRcontrol2; // Set when pitch control starts + bool useLQR1; // Set to start roll control + bool useLQR2; // Set to start pitch control + bool do_standup; // Set to start standup + bool do_rollup; // Set to start rollup + bool usb_control; // User setting that causes communication over USB instead of WiFi + bool use_IMUs; // User setting that disables all computation routines that use IMUs + bool motor_test; // Set when motor control test is started + bool use_pivotAcc_CP; // User setting that adds pivot acc. of ddq4 to pose estimate + bool use_pivotAcc_WC_vel; // User setting that adds pivot acc. of dq1, dq2, dq3 to pose estimate + bool use_pivotAcc_WC_acc; // User setting that adds pivot acc. of ddq1, ddq2 to pose estimate + } FlagsStruct; + +void check_wifi(FlagsStruct *flags, uint16_t counter, uint16_t counter_lastwifi); +void check_control(FlagsStruct *flags, Commstruct *comm_ud); +void check_rates(FlagsStruct *flags, Commstruct *comm_ud, float max_rate_motor2); +void check_battery(FlagsStruct *flags, uint16_t *battery_voltage); +void check_upright(FlagsStruct *flags, Commstruct *comm_ud, float attitude[2]); +//void reset_checks(FlagsStruct *flags, Commstruct *comm_ud, uint8_t *buffer_wifi_in); +void check_tick_time(FlagsStruct *flags, uint16_t *sys_flag, uint16_t *tick_time); +void check_loop_time(uint16_t *sys_flag, uint16_t loop_time); + +#endif diff --git a/firmware/M2-on-wheelbot/inc/config.h b/firmware/M2-on-wheelbot/inc/config.h new file mode 100644 index 0000000..bb337e0 --- /dev/null +++ b/firmware/M2-on-wheelbot/inc/config.h @@ -0,0 +1,116 @@ +#ifndef IMUTEST_CONFIG +#define IMUTEST_CONFIG +/************************************************************************** + * + * SETTINGS + * + **************************************************************************/ +#define INIT_WAIT 100 // ms to wait after init. each IMU +#define LOOP_FREQ 100 // desired polling freq [Hz] of IMUs +#define TIME_STEP (1/LOOP_FREQ) // desired polling freq [Hz] of IMUs +#define CALIB_NUM_SAMPLES 20 // amount of samples used for calibrating gyro + accelerometer. +#define NUM_SENSORS 4 // number of sensors +#define NUM_DIM 3 +#define NUM_IMU_DATA 6 + +/* MOTHERBOARD PIN ASSIGNMENT + * 0: Motherboard as used on Wheelbot v2 + * 3: New motherboard as designed by Naomi and detailed in Confluence + */ + +#define PCB_VERSION 0 + +#if PCB_VERSION == 3 + #define IMU_A PIN_D6 // pin of IMU 1 (closest to hinge) + #define IMU_B PIN_D7 // pin of IMU 2 (furthest to hinge) + #define IMU_C PIN_F6 + #define IMU_D PIN_F7 + #define BATTERY_VOLTAGE PIN_B4 // 34V max, 5V ADC max, 1bit=23.5mV + #define MIN_BATTERY_VOLTAGE 22.4 * 30.09 + #define MD PIN_D4 // uDriver CS pin + //#define MP PIN_D5 // Power switch to udriver +#endif + +#if PCB_VERSION == 0 + #define IMU_A PIN_D6 // pin of IMU 1 (closest to hinge) + #define IMU_B PIN_D7 // pin of IMU 2 (furthest to hinge) + #define IMU_C PIN_D3 + #define IMU_D PIN_D4 + #define BATTERY_VOLTAGE PIN_FO // 34V max, 5V ADC max, 1bit=23.5mV + #define MIN_BATTERY_VOLTAGE 22.4 * 28.0 + #define MD PIN_D5 // uDriver CS pin +#endif + +#define MOTOR_KV 160.0 // kv of motor +#define MOTOR_V 24.5 // nominal voltage of motor +#define MOTOR_TORQUE_CONSTANT 0.075 //0.06//0.075 //0.09375//(0.050259456) // kt of motor +#define FUSION_TUNING_PARAMETER 0.02 // tuning parameter for complementary filter + + +#define NOPE 3000 // Treshold at which gyro values are discarded +#define NOPE2 52 // Treshold at which gyro values are discarded + +// ranges for gyro and accel +//#define accel_r ACCEL_2G +//#define gyro_r GYRO_500DPS + +// filters for gyro and accel +//#define accel_f ACC_LPF_5HZ +//#define gyro_f GY_LPF_41HZ + +#define MAX_CURRENT_MOTOR 20.0 // Max current in Amps, the MN6007 KV160 has a max. peak current of 18A +#define MAX_CURRENT_LQR 15.0 +#define MAX_RATE_MOTOR1 330.0 // Max rate in rad/s, 3150 RPM +// The max. motor rate of the MN6007 Kv160 is 3800 RPM +// 398 rad/s = 3800 RPM +// 314 rad/s = 3000 RPM +// 210 rad/s = 2000 RPM +// 10 rad/s = 95 RPM +#define INIT_ANGLE -0.524 //-0.524 +#define Rw 0.049 // Wheel radius used for pivot point acc. computation + +#define DATA_MODE 1 + +/* DATA_MODE + * 1: CONTROL PLOTS -> FLAG_CONTROL = ? + * 2: PIVOT ACC PLOTS + * 3: IMU acc + * 4: IMU rate + * */ + +/* FLAG_PLOT chooses which additional data to send to PC + * 1: MISC + * 2: sys_flag, q[4] - init_q5_est, q[3] - init_q4_est + * 3: sys_flag, loop_time, + * 6: acc_pivot_B[0], comm_ud.velocityMotor2, comm_ud.currentTargetMotor2 + * 7: sys_flag, roll_bias, pitch_bias + * 8: data_in[0], data + * 9: sys_flag, comm_ud.currentMotor1, comm_ud.currentMotor2 + * 10: dq[2], q_acc[0], q_acc[1] + * 11: acc_pivot_B[0], acc_pivot_B[1], acc_pivot_B[2] + * 12: + * 13: imu_data[3][0], acc_pivot_B[0], comm_ud.velocityMotor2 + */ + +#define FLAG_PLOT 9 // 9 + +/************************************************************************** + * + * Constants + * + **************************************************************************/ +#define F_CPU 16000000 // freq of CPU +#define GRAV 9.81 +#define PI 3.14159265358979 +#define PI_2 1.57079632679 // pi/2 +#define STATIC_INIT_VALUE 70000 +#define MD_PACKET_SIZE 34 + +// Setup wifi comm +#define CHANNEL 1 +#define RXADDRESS 0xF4 +#define TXADDRESS 0x54 +#define PACKET_LENGTH 32 +#define PACKET_LENGTH_IN 32 // Note that m_rf_open() requires PACKET_LENGTH = PACKET_LENGTH_IN + +#endif // IMUTEST_CONFIG_H diff --git a/firmware/M2-on-wheelbot/inc/control.h b/firmware/M2-on-wheelbot/inc/control.h new file mode 100644 index 0000000..5c39ce1 --- /dev/null +++ b/firmware/M2-on-wheelbot/inc/control.h @@ -0,0 +1,37 @@ +#ifndef UNIWHEEL_MAEVARM_M2_CONTROL +#define UNIWHEEL_MAEVARM_M2_CONTROL +/************************************************************************** + * + * Header Files to include + * + **************************************************************************/ +/* subystem header files */ +#include "matrix_calculations.h" +#include "checks.h" + +/* global program files */ +#include "m_general.h" +#include "config.h" + +/* External header files */ +//#include + +/************************************************************************** + * + * Function declarations + * + **************************************************************************/ +void count_start(uint16_t *counter, uint16_t *counter_tmp, uint16_t *counts); +void count(uint16_t *counter, uint16_t *counter_tmp, uint16_t *counts); +(*counter_ptr)(uint16_t *counter, uint16_t *counter_tmp, uint16_t *counts); + +float saturate_torque(float torque, float omega, float dCurrent); +float return_current_roll(float q1, float q1_d, float int_q5d, float q5d); +float return_current_pitch(float q2, float q2_d, float int_q4_d, float q4_d); + +float test_motor(FlagsStruct *flags, uint16_t counter, float *time_test_start); + +void feedforward_signal(float *current, bool* flag_ptr, uint16_t *counter, uint16_t *counter_tmp, + uint16_t *counts, uint16_t *durations, float *controls, + uint16_t ramp_length1, uint16_t ramp_length2); +#endif //UNIWHEEL_MAEVARM_M2_CONTROL \ No newline at end of file diff --git a/firmware/M2-on-wheelbot/inc/icm20948.h b/firmware/M2-on-wheelbot/inc/icm20948.h new file mode 100644 index 0000000..aa0989a --- /dev/null +++ b/firmware/M2-on-wheelbot/inc/icm20948.h @@ -0,0 +1,196 @@ +#ifndef icm20948__ +#define icm20948__ + +#include +#include "m_general.h" +#include "m_spi.h" +//#include "m_usb.h" +#include "config.h" + +typedef enum +{ + ACCEL_2G = (0b00 << 1), + ACCEL_4G = (0b01 << 1), + ACCEL_8G = (0b10 << 1), + ACCEL_16G = (0b11 << 1) +} a_range_t; + +typedef enum +{ + GYRO_250DPS = (0b00 << 1), + GYRO_500DPS = (0b01 << 1), + GYRO_1000DPS = (0b10 << 1), + GYRO_2000DPS = (0b11 << 1) +} g_range_t; + +typedef enum +{ + ACC_LPF_1046HZ = 8, + ACC_LPF_420HZ = 7, + ACC_LPF_218HZ_B = 0, + ACC_LPF_218HZ = 1, + ACC_LPF_99HZ = 2, + ACC_LPF_45HZ = 3, + ACC_LPF_21HZ = 4, + ACC_LPF_10HZ = 5, + ACC_LPF_5HZ = 6 +} lpf_accel_bw_t; + +typedef enum +{ + GY_LPF_8800HZ = 8, + GY_LPF_3600HZ_HISPD = 16, + GY_LPF_3600HZ = 7, + GY_LPF_250HZ = 0, + GY_LPF_184HZ = 1, + GY_LPF_92HZ = 2, + GY_LPF_41HZ = 3, + GY_LPF_20HZ = 4, + GY_LPF_10HZ = 5, + GY_LPF_5HZ = 6 +} lpf_gyro_bw_t; + +// PUBLIC FUNCTIONS +bool icm20948_init(m2_gpio_t cs_pin, a_range_t accel_range, g_range_t gyro_range); +void icm20948_set_accel_range(m2_gpio_t cs_pin, a_range_t accel_range); +void icm20948_set_gyro_range(m2_gpio_t cs_pin, g_range_t gyro_range); +void icm20948_get_raw_3dof(m2_gpio_t cs_pin, int16_t *buffer); +void icm20948_get_raw_6dof(m2_gpio_t cs_pin, int16_t *buffer_acc, int16_t *buffer_gyro); +void icm20948_calib_accel(m2_gpio_t *cs_pin, volatile int16_t *accel_adjust, int16_t *acc_ref0); + +// BANK 0 REGISTERS +#define WHO_AM_I 0x00 +#define USER_CTRL 0x03 +#define PWR_MGMT_1 0x06 +#define PWR_MGMT_2 0x07 +#define ACCEL_XOUT_H 0x2D +#define ACCEL_XOUT_L 0x2E +#define ACCEL_YOUT_H 0x2F +#define ACCEL_YOUT_L 0x30 +#define ACCEL_ZOUT_H 0x31 +#define ACCEL_ZOUT_L 0x32 +#define GYRO_XOUT_H 0x33 +#define GYRO_XOUT_L 0x34 +#define GYRO_YOUT_H 0x35 +#define GYRO_YOUT_L 0x36 +#define GYRO_ZOUT_H 0x37 +#define GYRO_ZOUT_L 0x38 +#define REG_BANK_SEL 0x7F + +// BANK 2 REGISTERS +#define ACCEL_CONFIG 0x14 +#define GYRO_CONFIG 0x01 + +// VALUES +#define WHOAMI_VALUE 0xEA + +#define BANK_0 0x00 +#define BANK_1 0x10 +#define BANK_2 0x20 +#define BANK_3 0x30 + + +/* + // MPU9250 +#define ACCEL_OUT 0x3B +#define TEMP_OUT 0x41 +#define GYRO_OUT 0x43 +#define EXT_SENS_DATA_00 0x49 + +#define SMPDIV 0x19 +#define CONFIG 0x1A +#define ACCEL_CONFIG2 0x1D +#define INT_PIN_CFG 0x37 +#define INT_ENABLE 0x38 +#define INT_DISABLE 0x00 +#define INT_PULSE_50US 0x00 +#define INT_WOM_EN 0x40 +#define INT_RAW_RDY_EN 0x01 +#define PWR_CYCLE 0x20 +#define DIS_GYRO 0x07 + +#define I2C_MST_EN 0x20 +#define I2C_MST_CLK 0x0D +#define I2C_MST_CTRL 0x24 +#define I2C_SLV0_ADDR 0x25 +#define I2C_SLV0_REG 0x26 +#define I2C_SLV0_DO 0x63 +#define I2C_SLV0_CTRL 0x27 +#define I2C_SLV0_EN 0x80 +#define I2C_READ_FLAG 0x80 +#define MOT_DETECT_CTRL 0x69 +#define ACCEL_INTEL_EN 0x80 +#define ACCEL_INTEL_MODE 0x40 +#define LP_ACCEL_ODR 0x1E +#define WOM_THR 0x1F +#define FIFO_EN 0x23 +#define FIFO_TEMP 0x80 +#define FIFO_GYRO 0x70 +#define FIFO_ACCEL 0x08 +#define FIFO_MAG 0x01 +#define FIFO_COUNT 0x72 +#define FIFO_READ 0x74 + +#define AK8963_I2C_ADDR 0x0C +#define AK8963_HXL 0x03 +#define AK8963_CNTL1 0x0A +#define AK8963_PWR_DOWN 0x00 +#define AK8963_CNT_MEAS1 0x12 +#define AK8963_CNT_MEAS2 0x16 +#define AK8963_FUSE_ROM 0x0F +#define AK8963_CNTL2 0x0B +#define AK8963_RESET 0x01 +#define AK8963_ASA 0x10 +#define AK8963_WHO_AM_I 0x00 +*/ + +/* + // VARIABLES + extern uint8_t _buffer[]; + + float _accel_scale[NUM_IMU]; + float _gyro_scale[NUM_IMU]; + float _mag_scale_x[NUM_IMU]; + float _mag_scale_y[NUM_IMU]; + float _mag_scale_z[NUM_IMU]; + + a_range_t _accel_range[NUM_IMU]; + g_range_t _gyro_range[NUM_IMU]; + + uint8_t _fchoice_accel[NUM_IMU]; + uint8_t _fchoice_gyro[NUM_IMU]; + + lpf_accel_bw_t _accel_lpf_bandwidth[NUM_IMU]; + lpf_gyro_bw_t _gyro_lpf_bandwidth[NUM_IMU]; + + uint8_t _srd[NUM_IMU]; + + int32_t _gx_bias[NUM_IMU]; + int32_t _gy_bias[NUM_IMU]; + int32_t _gz_bias[NUM_IMU]; + + + // FUNCTIONS + void m_icm20948_init(); + void m_mpu9250_set_accel(uint8_t, a_range_t); + void m_mpu9250_set_gyro(uint8_t, g_range_t); + void m_mpu9250_set_accel_lpf(uint8_t, lpf_accel_bw_t); + void m_mpu9250_set_gyro_lpf(uint8_t, lpf_gyro_bw_t); + void m_mpu9250_fast_mode(uint8_t); + void m_mpu9250_fifo_on(uint8_t); + void m_read_spi_mag_registers(m2_gpio_t, uint8_t, uint8_t, uint8_t*); + void m_write_spi_mag_register(m2_gpio_t, uint8_t, uint8_t); + void m_read_spi_registers(m2_gpio_t, uint8_t, uint8_t, uint8_t*); + + + // PRIVATE FUNCTIONS + void _m_ak8963_init(); + void _m_ak8963_init_1(uint8_t); + void _m_ak8963_init_2(uint8_t); + void _m_ak8963_init_3(uint8_t); + void _m_ak8963_init_4(uint8_t); + void _m_mpu9250_calibrate_gyro(uint8_t); + void _blink_yes_or_no(bool); + */ + +#endif diff --git a/firmware/M2-on-wheelbot/inc/m_bus.h b/firmware/M2-on-wheelbot/inc/m_bus.h new file mode 100644 index 0000000..cb563eb --- /dev/null +++ b/firmware/M2-on-wheelbot/inc/m_bus.h @@ -0,0 +1,59 @@ +// ----------------------------------------------------------------------------- +// M2 data bus subsystem +// version: 2.0 +// date: June 30, 2011 +// author: J. Fiene +// ----------------------------------------------------------------------------- + +#ifndef m_bus__ +#define m_bus__ + +#include "m_general.h" +#include + +#define MAX_WAIT 1000 +#define INTERPACKET 10 +#define READ 1 +#define WRITE 0 + +// ----------------------------------------------------------------------------- +// Public functions: +// ----------------------------------------------------------------------------- + +void m_bus_init(void); +// FUNCTIONALITY: +// initialize the M2 data bus, which uses pins D0-D2 and is available through the +// 5-pin end header. When new data is available from a slave, the INT2_vect interrupt +// will be triggered, and you must act accordingly! +// +// TAKES: +// nothing +// +// RETURNS: +// nothing + +unsigned char m_read_register(unsigned char addr, unsigned char reg); +// FUNCTIONALITY: +// sends [START + W] [register address] [STOP] [START + R] +// +// TAKES: +// addr - I2C slave address +// reg - register address +// +// RETURNS: +// register value + +unsigned char m_write_register(unsigned char addr, unsigned char reg, unsigned char value); +// FUNCTIONALITY: +// sends [START + W] [register address] [value] [STOP] +// +// TAKES: +// addr - I2C slave address +// reg - register address +// value - value to place in register +// +// RETURNS: +// 1 - success +// 0 - communication error + +#endif \ No newline at end of file diff --git a/firmware/M2-on-wheelbot/inc/m_crc32.h b/firmware/M2-on-wheelbot/inc/m_crc32.h new file mode 100644 index 0000000..cb61c86 --- /dev/null +++ b/firmware/M2-on-wheelbot/inc/m_crc32.h @@ -0,0 +1,9 @@ +#pragma once + +#include "m_general.h" + + +// EXTERNAL FUNCTIONS + +uint32_t m_crc32 (uint8_t *buffer, int length); + diff --git a/firmware/M2-on-wheelbot/inc/m_general.h b/firmware/M2-on-wheelbot/inc/m_general.h new file mode 100644 index 0000000..b42de46 --- /dev/null +++ b/firmware/M2-on-wheelbot/inc/m_general.h @@ -0,0 +1,100 @@ +// ----------------------------------------------------------------------------- +// MAEVARM general macros +// version: 2.1 +// date: Sept 29, 2011 +// author: J. Fiene +// ----------------------------------------------------------------------------- +#ifndef m_general__ +#define m_general__ + +// ----------------------------------------------------------------------------- +// Useful pre-compile constants +// ----------------------------------------------------------------------------- + +#define TRUE 1 +#define FALSE 0 + +#define OFF 0 +#define ON 1 +#define TOGGLE 2 + +// ----------------------------------------------------------------------------- +// General AVR libraries that we'll need at times: +// ----------------------------------------------------------------------------- + +#include +#include +#include +#include +#include +#include +#include + + +// ----------------------------------------------------------------------------- +// Bit manipulation and validation: +// ----------------------------------------------------------------------------- + +#define set(reg,bit) reg |= (1<<(bit)) +#define clear(reg,bit) reg &= ~(1<<(bit)) +#define toggle(reg,bit) reg ^= (1<<(bit)) + +#define check(reg,bit) (bool)(reg & (1<<(bit))) +// As of version 2.0, this will return either 1 (TRUE) or 0 (FALSE) + + +// ----------------------------------------------------------------------------- +// Disable JTAG to access F4-F7: +// ----------------------------------------------------------------------------- + +#define m_disableJTAG() MCUCR = (1 << JTD); MCUCR = (1 << JTD) +// Setting the JTD bit in MCUCR twice within four clock cycles will allow user +// access to F4-F7 as normal port pins. Note that using |= is too slow for this +// operation to work correctly, so we are setting the entire register +// (forutnately, all other bits in MCUCR are 0 anyway). + + +// ----------------------------------------------------------------------------- +// Set the system clock: +// ----------------------------------------------------------------------------- + +#define m_clockdivide(val) CLKPR = (1< + +#define WRITE_FLAG 0x00 +#define READ_FLAG 0x80 + +#define CS_B0() set(DDRB, 0) +#define SELECT_B0() clear(PORTB, 0) +#define DESELECT_B0() set(PORTB, 0) + +#define CS_B4() set(DDRB, 4) +#define SELECT_B4() clear(PORTB, 4) +#define DESELECT_B4() set(PORTB, 4) + +#define CS_B5() set(DDRB, 5) +#define SELECT_B5() clear(PORTB, 5) +#define DESELECT_B5() set(PORTB, 5) + +#define CS_B6() set(DDRB, 6) +#define SELECT_B6() clear(PORTB, 6) +#define DESELECT_B6() set(PORTB, 6) + +#define CS_B7() set(DDRB, 7) +#define SELECT_B7() clear(PORTB, 7) +#define DESELECT_B7() set(PORTB, 7) + +#define CS_C6() set(DDRC, 6) +#define SELECT_C6() clear(PORTC, 6) +#define DESELECT_C6() set(PORTC, 6) + +#define CS_C7() set(DDRC, 7) +#define SELECT_C7() clear(PORTC, 7) +#define DESELECT_C7() set(PORTC, 7) + +#define CS_D0() set(DDRD, 0) +#define SELECT_D0() clear(PORTD, 0) +#define DESELECT_D0() set(PORTD, 0) + +#define CS_D1() set(DDRD, 1) +#define SELECT_D1() clear(PORTD, 1) +#define DESELECT_D1() set(PORTD, 1) + +#define CS_D2() set(DDRD, 2) +#define SELECT_D2() clear(PORTD, 2) +#define DESELECT_D2() set(PORTD, 2) + +#define CS_D3() set(DDRD, 3) +#define SELECT_D3() clear(PORTD, 3) +#define DESELECT_D3() set(PORTD, 3) + +#define CS_D4() set(DDRD, 4) +#define SELECT_D4() clear(PORTD, 4) +#define DESELECT_D4() set(PORTD, 4) + +#define CS_D5() set(DDRD, 5) +#define SELECT_D5() clear(PORTD, 5) +#define DESELECT_D5() set(PORTD, 5) + +#define CS_D6() set(DDRD, 6) +#define SELECT_D6() clear(PORTD, 6) +#define DESELECT_D6() set(PORTD, 6) + +#define CS_D7() set(DDRD, 7) +#define SELECT_D7() clear(PORTD, 7) +#define DESELECT_D7() set(PORTD, 7) + +#define CS_E6() set(DDRE, 6) +#define SELECT_E6() clear(PORTE, 6) +#define DESELECT_E6() set(PORTE, 6) + +#define CS_F0() set(DDRF, 0) +#define SELECT_F0() clear(PORTF, 0) +#define DESELECT_F0() set(PORTF, 0) + +#define CS_F1() set(DDRF, 1) +#define SELECT_F1() clear(PORTF, 1) +#define DESELECT_F1() set(PORTF, 1) + +#define CS_F4() set(DDRF, 4) +#define SELECT_F4() clear(PORTF, 4) +#define DESELECT_F4() set(PORTF, 4) + +#define CS_F5() set(DDRF, 5) +#define SELECT_F5() clear(PORTF, 5) +#define DESELECT_F5() set(PORTF, 5) + +#define CS_F6() set(DDRF, 6) +#define SELECT_F6() clear(PORTF, 6) +#define DESELECT_F6() set(PORTF, 6) + +#define CS_F7() set(DDRF, 7) +#define SELECT_F7() clear(PORTF, 7) +#define DESELECT_F7() set(PORTF, 7) + +#ifndef CS_SETUP +#define CS_SETUP(cs) \ +do { \ +switch (cs) \ +{ \ +case PIN_B0: \ +DESELECT_B0(); \ +CS_B0(); \ +break; \ +case PIN_B4: \ +DESELECT_B4(); \ +CS_B4(); \ +break; \ +case PIN_B5: \ +DESELECT_B5(); \ +CS_B5(); \ +break; \ +case PIN_B6: \ +DESELECT_B6(); \ +CS_B6(); \ +break; \ +case PIN_B7: \ +DESELECT_B7(); \ +CS_B7(); \ +break; \ +case PIN_C6: \ +DESELECT_C6(); \ +CS_C6(); \ +break; \ +case PIN_C7: \ +DESELECT_C7(); \ +CS_C7(); \ +break; \ +case PIN_D0: \ +DESELECT_D0(); \ +CS_D0(); \ +break; \ +case PIN_D1: \ +DESELECT_D1(); \ +CS_D1(); \ +break; \ +case PIN_D2: \ +DESELECT_D2(); \ +CS_D2(); \ +break; \ +case PIN_D3: \ +DESELECT_D3(); \ +CS_D3(); \ +break; \ +case PIN_D4: \ +DESELECT_D4(); \ +CS_D4(); \ +break; \ +case PIN_D5: \ +DESELECT_D5(); \ +CS_D5(); \ +break; \ +case PIN_D6: \ +DESELECT_D6(); \ +CS_D6(); \ +break; \ +case PIN_D7: \ +DESELECT_D7(); \ +CS_D7(); \ +break; \ +case PIN_E6: \ +DESELECT_E6(); \ +CS_E6(); \ +break; \ +case PIN_F0: \ +DESELECT_F0(); \ +CS_F0(); \ +break; \ +case PIN_F1: \ +DESELECT_F1(); \ +CS_F1(); \ +break; \ +case PIN_F4: \ +DESELECT_F4(); \ +CS_F4(); \ +break; \ +case PIN_F5: \ +DESELECT_F5(); \ +CS_F5(); \ +break; \ +case PIN_F6: \ +DESELECT_F6(); \ +CS_F6(); \ +break; \ +case PIN_F7: \ +DESELECT_F7(); \ +CS_F7(); \ +break; \ +} \ +} while (0) +#endif // CHIP_SELECT + + +#ifndef CHIP_SELECT +#define CHIP_SELECT(cs) \ +do { \ +switch (cs) \ +{ \ +case PIN_B0: \ +SELECT_B4(); \ +break; \ +case PIN_B4: \ +SELECT_B4(); \ +break; \ +case PIN_B5: \ +SELECT_B5(); \ +break; \ +case PIN_B6: \ +SELECT_B6(); \ +break; \ +case PIN_B7: \ +SELECT_B7(); \ +break; \ +case PIN_C6: \ +SELECT_C6(); \ +break; \ +case PIN_C7: \ +SELECT_C7(); \ +break; \ +case PIN_D0: \ +SELECT_D0(); \ +break; \ +case PIN_D1: \ +SELECT_D1(); \ +break; \ +case PIN_D2: \ +SELECT_D2(); \ +break; \ +case PIN_D3: \ +SELECT_D3(); \ +break; \ +case PIN_D4: \ +SELECT_D4(); \ +break; \ +case PIN_D5: \ +SELECT_D5(); \ +break; \ +case PIN_D6: \ +SELECT_D6(); \ +break; \ +case PIN_D7: \ +SELECT_D7(); \ +break; \ +case PIN_E6: \ +SELECT_E6(); \ +break; \ +case PIN_F0: \ +SELECT_F0(); \ +break; \ +case PIN_F1: \ +SELECT_F1(); \ +break; \ +case PIN_F4: \ +SELECT_F4(); \ +break; \ +case PIN_F5: \ +SELECT_F5(); \ +break; \ +case PIN_F6: \ +SELECT_F6(); \ +break; \ +case PIN_F7: \ +SELECT_F7(); \ +break; \ +} \ +} while (0) +#endif // CHIP_SELECT + +#ifndef CHIP_DESELECT +#define CHIP_DESELECT(cs) \ +do { \ +switch (cs) \ +{ \ +case PIN_B0: \ +DESELECT_B0(); \ +break; \ +case PIN_B4: \ +DESELECT_B4(); \ +break; \ +case PIN_B5: \ +DESELECT_B5(); \ +break; \ +case PIN_B6: \ +DESELECT_B6(); \ +break; \ +case PIN_B7: \ +DESELECT_B7(); \ +break; \ +case PIN_C6: \ +DESELECT_C6(); \ +break; \ +case PIN_C7: \ +DESELECT_C7(); \ +break; \ +case PIN_D0: \ +DESELECT_D0(); \ +break; \ +case PIN_D1: \ +DESELECT_D1(); \ +break; \ +case PIN_D2: \ +DESELECT_D2(); \ +break; \ +case PIN_D3: \ +DESELECT_D3(); \ +break; \ +case PIN_D4: \ +DESELECT_D4(); \ +break; \ +case PIN_D5: \ +DESELECT_D5(); \ +break; \ +case PIN_D6: \ +DESELECT_D6(); \ +break; \ +case PIN_D7: \ +DESELECT_D7(); \ +break; \ +case PIN_E6: \ +DESELECT_E6(); \ +break; \ +case PIN_F0: \ +DESELECT_F0(); \ +break; \ +case PIN_F1: \ +DESELECT_F1(); \ +break; \ +case PIN_F4: \ +DESELECT_F4(); \ +break; \ +case PIN_F5: \ +DESELECT_F5(); \ +break; \ +case PIN_F6: \ +DESELECT_F6(); \ +break; \ +case PIN_F7: \ +DESELECT_F7(); \ +break; \ +} \ +} while (0) +#endif // CHIP_DESELECT + + +typedef enum +{ + SPI_125KHZ = 0, + SPI_250KHZ, + SPI_500KHZ, + SPI_1MHZ, + SPI_2MHZ, + SPI_4MHZ, + SPI_8MHZ +} spi_freq_t; + +typedef enum +{ + PIN_B0, + PIN_B4, + PIN_B5, + PIN_B6, + PIN_B7, + PIN_C6, + PIN_C7, + PIN_D0, + PIN_D1, + PIN_D2, + PIN_D3, + PIN_D4, + PIN_D5, + PIN_D6, + PIN_D7, + PIN_E6, + PIN_F0, + PIN_F1, + PIN_F4, + PIN_F5, + PIN_F6, + PIN_F7 +} m2_gpio_t; + +spi_freq_t _spi_freq; + +// EXTERNAL FUNCTIONS +void m_spi_init(); +void m_spi_cs_setup(m2_gpio_t cs_pin); +void m_spi_speed(spi_freq_t); +void m_spi_write_register(m2_gpio_t cs_pin, uint8_t reg, uint8_t val); +uint8_t m_spi_read_register(m2_gpio_t cs_pin, uint8_t reg); +void m_spi_read_registers(m2_gpio_t cs_pin, uint8_t start_reg, uint8_t count, uint8_t *dest); +void m_spi_shift_buffers(m2_gpio_t cs_pin, uint8_t *tx, uint8_t *rx, uint16_t bytes); +void m_spi_shift_MD(m2_gpio_t cs_pin, uint8_t *tx, uint8_t *rx, uint8_t bytes); + + +// INTERNAL FUNCTIONS +uint8_t read_spi_byte(); +void write_spi_byte(uint8_t); +uint8_t exchange_spi_byte(uint8_t); + +#endif diff --git a/firmware/M2-on-wheelbot/inc/m_usb.h b/firmware/M2-on-wheelbot/inc/m_usb.h new file mode 100644 index 0000000..96e11be --- /dev/null +++ b/firmware/M2-on-wheelbot/inc/m_usb.h @@ -0,0 +1,189 @@ +// ----------------------------------------------------------------------------- +// M2 USB communication subsystem +// version: 2.1 +// date: June 16, 2012 +// authors: J. Fiene & J. Romano +// ----------------------------------------------------------------------------- + +#ifndef m_usb__ +#define m_usb__ + +#include "m_general.h" +#include + + +// ----------------------------------------------------------------------------- +// Public functions: +// ----------------------------------------------------------------------------- + +// INITIALIZATION: ------------------------------------------------------------- + +void m_usb_init(void); +// initialize the USB subsystem + +char m_usb_isconnected(void); +// confirm that the USB port is connected to a PC + + +// RECEIVE: ------------------------------------------------------------------- + +unsigned char m_usb_rx_available(void); +// returns the number of bytes (up to 255) waiting in the receive FIFO buffer + +char m_usb_rx_char(void); +// retrieve a oldest byte from the receive FIFO buffer (-1 if timeout/error) + +void m_usb_rx_flush(void); +// discard all data in the receive buffer + + + +// TRANSMIT: ------------------------------------------------------------------ + +char m_usb_tx_char(unsigned char c); +// add a single 8-bit unsigned char to the transmit buffer, return -1 if error + +void m_usb_tx_hexchar(unsigned char i); +// add an unsigned char to the transmit buffer, send as two hex-value characters + +void m_usb_tx_hex(unsigned int i); +// add an unsigned int to the transmit buffer, send as four hex-value characters + +void m_usb_tx_int(int i); +// add a signed int to the transmit buffer, send as a sign character then 5 decimal-value characters + +void m_usb_tx_uint(unsigned int i); +// add an unsigned int to the transmit buffer, send as 5 decimal-value characters + +void m_usb_tx_long(long i); +// add a signed long to the transmit buffer, send as a sign character then 5 decimal-value characters + +void m_usb_tx_ulong(unsigned long i); +// add an unsigned long to the transmit buffer, send as 5 decimal-value characters + +#define m_usb_tx_string(s) print_P(PSTR(s)) +// add a string to the transmit buffer + + + +// ----------------------------------------------------------------------------- +// ----------------------------------------------------------------------------- +// ----------------------------------------------------------------------------- + +// ---- OVERLOADS FOR M1 BACK COMPATIBILITY ---- +#define usb_init() m_usb_init() +#define usb_configured() m_usb_isconnected() + +#define usb_rx_available() m_usb_rx_available() +#define usb_rx_flush() m_usb_rx_flush() +#define usb_rx_char() m_usb_rx_char() + +#define usb_tx_char(val) m_usb_tx_char(val) +#define usb_tx_hex(val) m_usb_tx_hex(val) +#define usb_tx_decimal(val) m_usb_tx_uint(val) +#define usb_tx_string(val) m_usb_tx_string(val) +#define usb_tx_push() m_usb_tx_push() + +#define m_usb_rx_ascii() m_usb_rx_char() +#define m_usb_tx_ascii(val) m_usb_tx_char(val) + + +// EVERYTHING ELSE ***************************************************************** + +// setup + +int8_t usb_serial_putchar(uint8_t c); // transmit a character +int8_t usb_serial_putchar_nowait(uint8_t c); // transmit a character, do not wait +int8_t usb_serial_write(const uint8_t *buffer, uint16_t size); // transmit a buffer +void print_P(const char *s); +void phex(unsigned char c); +void phex16(unsigned int i); +void m_usb_tx_hex8(unsigned char i); +void m_usb_tx_push(void); + + +// serial parameters +uint32_t usb_serial_get_baud(void); // get the baud rate +uint8_t usb_serial_get_stopbits(void); // get the number of stop bits +uint8_t usb_serial_get_paritytype(void);// get the parity type +uint8_t usb_serial_get_numbits(void); // get the number of data bits +uint8_t usb_serial_get_control(void); // get the RTS and DTR signal state +int8_t usb_serial_set_control(uint8_t signals); // set DSR, DCD, RI, etc + +// constants corresponding to the various serial parameters +#define USB_SERIAL_DTR 0x01 +#define USB_SERIAL_RTS 0x02 +#define USB_SERIAL_1_STOP 0 +#define USB_SERIAL_1_5_STOP 1 +#define USB_SERIAL_2_STOP 2 +#define USB_SERIAL_PARITY_NONE 0 +#define USB_SERIAL_PARITY_ODD 1 +#define USB_SERIAL_PARITY_EVEN 2 +#define USB_SERIAL_PARITY_MARK 3 +#define USB_SERIAL_PARITY_SPACE 4 +#define USB_SERIAL_DCD 0x01 +#define USB_SERIAL_DSR 0x02 +#define USB_SERIAL_BREAK 0x04 +#define USB_SERIAL_RI 0x08 +#define USB_SERIAL_FRAME_ERR 0x10 +#define USB_SERIAL_PARITY_ERR 0x20 +#define USB_SERIAL_OVERRUN_ERR 0x40 + +// This file does not include the HID debug functions, so these empty +// macros replace them with nothing, so users can compile code that +// has calls to these functions. +#define usb_debug_putchar(c) +#define usb_debug_flush_output() + +#define EP_TYPE_CONTROL 0x00 +#define EP_TYPE_BULK_IN 0x81 +#define EP_TYPE_BULK_OUT 0x80 +#define EP_TYPE_INTERRUPT_IN 0xC1 +#define EP_TYPE_INTERRUPT_OUT 0xC0 +#define EP_TYPE_ISOCHRONOUS_IN 0x41 +#define EP_TYPE_ISOCHRONOUS_OUT 0x40 +#define EP_SINGLE_BUFFER 0x02 +#define EP_DOUBLE_BUFFER 0x06 +#define EP_SIZE(s) ((s) == 64 ? 0x30 : \ + ((s) == 32 ? 0x20 : \ + ((s) == 16 ? 0x10 : \ + 0x00))) + +#define MAX_ENDPOINT 4 + +#define LSB(n) (n & 255) +#define MSB(n) ((n >> 8) & 255) + +#define HW_CONFIG() (UHWCON = 0x01) + +#ifdef M1 + #define PLL_CONFIG() (PLLCSR = 0x02) // fixed to 8MHz clock +#else + #define PLL_CONFIG() (PLLCSR = 0x12) // 0001 0010 For a 16MHz clock +#endif + +#define USB_CONFIG() (USBCON = ((1< + +/************************************************************************** + * + * Function declarations + * + **************************************************************************/ +void estimate_pivot_acc(float *acc_pivot_B, float *q, float *dq, float ddq4, FlagsStruct *flags); +void estimate_pivot_acc_linearized(float *acc_pivot_B, float *q, float *dq, float ddq4, FlagsStruct *flags); + +void attitude_estimate_accel(float *q_acc, float *acc_pivot_B, int16_t *imu_data); +void compute_estimator_bias(FlagsStruct *flags, uint16_t counter, uint16_t *counter_tmp, float *roll_bias, + float *roll_bias_sum, float *pitch_bias, float *pitch_bias_sum, float attitude[2]); + +int16_t nope_func(int16_t new_value, int16_t prev_value, bool *imu_check); +int16_t nope_func2(int16_t new_value, int16_t prev_value, bool *imu_check); +int16_t nope_func3(int16_t new_value, int16_t prev_value); + +float threshold_func(float value, float prev_value, float boundary); + +//void body_to_euler(float *T, float *body_rate); +float ApproxAtan(float z); +float ApproxAtan2(float y, float x); + +#endif diff --git a/firmware/M2-on-wheelbot/inc/udriver.h b/firmware/M2-on-wheelbot/inc/udriver.h new file mode 100644 index 0000000..4231126 --- /dev/null +++ b/firmware/M2-on-wheelbot/inc/udriver.h @@ -0,0 +1,93 @@ +#ifndef UNIWHEEL_MAEVARM_M2_UDRIVER +#define UNIWHEEL_MAEVARM_M2_UDRIVER +/************************************************************************** + * + * Header Files to include + * + **************************************************************************/ +/* subystem header files */ +#include "m_crc32.h" +#include "m_spi.h" + +/* global program files */ +#include "m_general.h" +//#include "m_usb.h" +#include "config.h" + +/* External header files */ +#include + +/************************************************************************** + * + * typedef and enumerates + * + **************************************************************************/ +enum +{ + SYSTEM_ENABLE = 0x8000, + MOTOR_1_ENABLE = 0x4000, + MOTOR_2_ENABLE = 0x2000, + ROLLOVER_ENABLE = 0x1000, + MOTOR_1_INDEX_COMPENSATION_ENABLE = 0x0800, + MOTOR_2_INDEX_COMPENSATION_ENABLE = 0x0400, + SYSTEM_DISABLE = ~0x8000, + MOTOR_1_DISABLE = ~0x4000, + MOTOR_2_DISABLE = ~0x2000, + ROLLOVER_DISABLE = ~0x1000, + MOTOR_1_INDEX_COMPENSATION_DISABLE = ~0x0800, + MOTOR_2_INDEX_COMPENSATION_DISABLE = ~0x0400, +}; + +typedef struct { + // MOSI + float currentLimit1; + float currentTargetMotor1; + + float currentLimit2; + float currentTargetMotor2; + + // MISO + float currentMotor1; + float positionMotor1; + float velocityMotor1; + + float currentMotor2; + float positionMotor2; + float velocityMotor2; + + //bool motor1Ready; + //bool motor2Ready; + //bool systemEnabled; + + uint8_t status_errcode; // uDriver error messages + // 0: No error, 1: Encoder error, 2: CAN recieve timeout, 3: Critical temperature + bool status_SE; // systemeEnabled: udriver ON + bool status_M1E; // motor1Enabled: Motor 1 enabled + bool status_M1R; // Motor 1 is ready + bool status_IDX1D; // motor1IndexDetected: At least one encoder has been detected + bool status_IDX1T; // motor1IndexToggle: toggles for every encoder index + + bool status_M2E; + bool status_M2R; + bool status_IDX2D; + bool status_IDX2T; +} Commstruct; + +/************************************************************************** + * + * Function declarations + * + **************************************************************************/ +void setMode(uint16_t setting); +bool send_spi_udriver(Commstruct *comm_ud); +void check_motor_signs(Commstruct *pcomm, uint16_t *p_sys_flag, float *sign_motor1, float *sign_motor2, + bool (*send_spi_udriver)(Commstruct *)); + +void printState(); + +uint16_t flipu16(uint16_t val); +int16_t flip16(int16_t val); +uint32_t flipu32(uint32_t val); +int32_t flip32(int32_t val); + +#endif \ No newline at end of file diff --git a/firmware/M2-on-wheelbot/src/checks.c b/firmware/M2-on-wheelbot/src/checks.c new file mode 100644 index 0000000..5f97a2a --- /dev/null +++ b/firmware/M2-on-wheelbot/src/checks.c @@ -0,0 +1,100 @@ +#include "checks.h" + +void check_wifi(FlagsStruct *flags, uint16_t counter, uint16_t counter_lastwifi){ + // Check Wifi connection + if ( ( (counter - counter_lastwifi) > 150 ) && ( counter_lastwifi != 0 ) ) { + (*flags).wifi = FALSE; + } +} + +void check_control(FlagsStruct *flags, Commstruct *comm_ud){ + // Check motor currents + if ( fabs( (*comm_ud).currentMotor1 ) > MAX_CURRENT_MOTOR || + fabs( (*comm_ud).currentMotor2 ) > MAX_CURRENT_MOTOR ) { + (*flags).control = FALSE; + } +} + +void check_rates(FlagsStruct *flags, Commstruct *comm_ud, float max_rate_motor2) { + // Check motor rates + if (fabs( (*comm_ud).velocityMotor1 ) > MAX_RATE_MOTOR1 || + fabs( (*comm_ud).velocityMotor2 ) > max_rate_motor2 ) { + (*flags).rate = FALSE; + } +} + +void check_battery(FlagsStruct *flags, uint16_t *battery_voltage){ + // Check battery voltage + uint16_t battery_voltage_new; + + if( check( ADCSRA , ADIF ) ) { // when the conversion finishes + set( ADCSRA , ADIF ); // reset the flag + battery_voltage_new = ADCL; + battery_voltage_new |= (unsigned int)ADCH << 8; + *battery_voltage = 0.02 * battery_voltage_new + 0.98 * (*battery_voltage); // low pass filter filtering acceleration estimation + if ( (*battery_voltage) < MIN_BATTERY_VOLTAGE ) { + (*flags).battery = FALSE; + } + } +} + +void check_upright(FlagsStruct *flags, Commstruct *comm_ud, float attitude[2]){ + // Check Wheelbot reached upright, 0.35 DEBUG DEBUG + if ( ( (*flags).upright == FALSE ) && fabs(attitude[0]) < 0.2) { + (*flags).upright = TRUE; + } + + // Check roll angle, ( 20 degree = 0.35 rad ) + //if ( ( (*flags).upright == TRUE ) && ( fabs(attitude[0]) > 0.52 ) ) { + // (*flags).angle = FALSE; + //} + // Check pitch angle + //if ( fabs(attitude[1]) > 0.52 ) { + // (*flags).angle = FALSE; + //} +} + +/* +void reset_checks(FlagsStruct *flags, Commstruct *comm_ud, uint8_t *buffer_wifi_in){ + // Reset command + // Note: Control and current safety-flags should not be resetable + if (buffer_wifi_in[0] == 'x') + { + (*flags).upright = FALSE; + (*flags).system = TRUE; + (*flags).wifi = TRUE; + (*flags).angle = TRUE; + + if ( fabs( (*comm_ud).velocityMotor1) < MAX_RATE_MOTOR1 || + fabs( (*comm_ud).velocityMotor2) < MAX_RATE_MOTOR2 ) { + (*flags).rate = TRUE; + m_red( OFF ); + } + } +} +*/ + +void check_tick_time(FlagsStruct *flags, uint16_t *sys_flag, uint16_t *tick_time){ + *tick_time = micros(FALSE); + + if ( *tick_time > 10050 ) { // USE LOOP_FREQ in future + *sys_flag = 3; + m_red( ON ); + } + else if ( (*flags).loop_finished == FALSE ) { + *sys_flag = 4; + m_red( ON ); + } + + (*flags).loop_finished = FALSE; +} + +void check_loop_time(uint16_t *sys_flag, uint16_t loop_time){ + + if ( loop_time > 10000 ) { // USE LOOP_FREQ in future + *sys_flag = 5; + m_red( ON ); + } +} + + diff --git a/firmware/M2-on-wheelbot/src/control.c b/firmware/M2-on-wheelbot/src/control.c new file mode 100644 index 0000000..9eba387 --- /dev/null +++ b/firmware/M2-on-wheelbot/src/control.c @@ -0,0 +1,103 @@ +#include "control.h" + +float K_roll[4] = {4.5 , 0.25, 0.0003, 0.0018}; + +float K_pitch[4] = {1.6, 0.14, 0.04, 0.0344}; // great on tilted plane + + +void count_start(uint16_t *counter, uint16_t *counter_tmp, uint16_t *counts) +{ + *counter_tmp = *counter; + *counts = *counter - *counter_tmp; + counter_ptr = count; +} + + +void count(uint16_t *counter, uint16_t *counter_tmp, uint16_t *counts) +{ + *counts = *counter - *counter_tmp; +} + + +float saturate_current(float current) +{ + float output = 0; + if ( current >= MAX_CURRENT_LQR ) { + output = MAX_CURRENT_LQR; + } else if ( current <= ( -1.0 * MAX_CURRENT_LQR ) ) { + output = -1.0 * MAX_CURRENT_LQR; + } else { + output = current; + } + return output; +} + + +float return_current_roll(float q1, float q1_d, float int_q5d, float q5d) +{ + return (saturate_current((K_roll[0] * q1 + K_roll[1] * q1_d + + K_roll[2] * int_q5d + K_roll[3] * q5d) / MOTOR_TORQUE_CONSTANT)); +} // End return_current_roll + +float return_current_pitch(float q2, float q2_d, float int_q4_d, float q4_d) +{ + return (saturate_current((K_pitch[0] * q2 + K_pitch[1] * q2_d + + K_pitch[2] * int_q4_d + K_pitch[3] * q4_d) / MOTOR_TORQUE_CONSTANT)); +} // End return_current_pitch + + +float test_motor(FlagsStruct *flags, uint16_t counter, float *time_test_start) { + float time_in_sec = 0.0; + float max_time = 10.0; + float motor_current = 0.0; + float max_current = 6.0; + float singal_freq = 4.0; + + if ( (*flags).motor_test == FALSE ) + { + (*flags).motor_test = TRUE; + *time_test_start = counter / 100.0; + } + + time_in_sec = ( counter / 100.0 ) - *time_test_start; + + if ( time_in_sec < max_time) + { + motor_current = max_current * sin( (PI * time_in_sec) / (2.0 * max_time) ) + * sin( (singal_freq * 10.0 * PI * time_in_sec) / (2.0 * max_time) ) + + 3.0 * sin( (singal_freq * 100.0 * PI * time_in_sec) / (2.0 * max_time) ); + } + else + { + motor_current = 0.0; + } + + return motor_current; //motor_current; +} // End test_motor + + +void feedforward_signal(float *current, bool* flag_ptr, + uint16_t *counter, uint16_t *counter_tmp, uint16_t *counts, + uint16_t *durations, float *controls, uint16_t ramp_length1, uint16_t ramp_length2) +{ + + counter_ptr( counter, counter_tmp, counts ); + + if ( *counts < durations[0] ) { *current = controls[0]; } + else if ( *counts < durations[1] ) { *current = controls[1]; } + else if ( *counts < durations[2] ) { *current = controls[2]; } + //else if ( *counts < durations[3] ) { *current = controls[3]; } + else if ( *counts < ( durations[2] + ramp_length1 ) ) + { *current = controls[3] * sin( (PI_2/ramp_length1) * (*counts - durations[2]) ); } + else if ( *counts < durations[3] - ramp_length2 ) { *current = controls[3]; } + else if ( *counts < durations[3] ) + { *current = controls[3] * cos( (PI_2/ramp_length2) * (*counts - durations[3] + ramp_length2) ); } + else if ( *counts < ( durations[3] + 15 ) ) + { *current = controls[4] * sin( (PI_2/15) * (*counts - durations[3]) ); } + else if ( *counts < durations[4] ) { *current = controls[4]; } + else + { + *current = 0.0; + *flag_ptr = TRUE; + } +} \ No newline at end of file diff --git a/firmware/M2-on-wheelbot/src/icm20948.c b/firmware/M2-on-wheelbot/src/icm20948.c new file mode 100644 index 0000000..64804ee --- /dev/null +++ b/firmware/M2-on-wheelbot/src/icm20948.c @@ -0,0 +1,88 @@ +#include "icm20948.h" + +bool icm20948_init(m2_gpio_t cs_pin, a_range_t accel_range, g_range_t gyro_range){ + + // INITIALIZE THE SPI SUBSYSTEM (IT IS OKAY TO CALL MULTIPLE TIMES) + m_spi_init(); + + // ASSOCIATE THE CHIP-SELECT PIN + m_spi_cs_setup(cs_pin); + + // SET THE CLOCK SOURCE + m_spi_write_register(cs_pin, REG_BANK_SEL, BANK_0); // BANK 0 + m_wait(100); + m_spi_write_register(cs_pin, PWR_MGMT_1, 0b10000000); // RESET (7) + AUTO CLK (1) + m_wait(100); + m_spi_write_register(cs_pin, USER_CTRL, 0b00110000); //(1<<5)|(1<<4)); + m_wait(100); + m_spi_write_register(cs_pin, PWR_MGMT_1, 0x01); // AUTO CLK (1) + m_wait(100); + if(m_spi_read_register(cs_pin, WHO_AM_I) != WHOAMI_VALUE) { + return false; + } + + // CONFIGURE ACCEL & GYRO + m_spi_write_register(cs_pin, PWR_MGMT_2, 0x00); // ENABLE ACCEL & GYRO + icm20948_set_accel_range(cs_pin, accel_range); // SET ACCEL RANGE + icm20948_set_gyro_range(cs_pin, gyro_range); // SET GYRO RANGE + // low-pass filter + // sample-rate divider + // CONFIGURE MAGNETOMETER + + return true; +} + +void icm20948_set_accel_range(m2_gpio_t cs_pin, a_range_t accel_range){ + spi_freq_t current_freq = _spi_freq; // Slow down SPI as needed + if ((uint8_t)current_freq > (uint8_t)SPI_250KHZ) { m_spi_speed(SPI_250KHZ); } + m_spi_write_register(cs_pin, REG_BANK_SEL, BANK_2); // switch to bank 2 + m_spi_write_register(cs_pin, ACCEL_CONFIG, accel_range); + m_spi_write_register(cs_pin, REG_BANK_SEL, BANK_0); // switch to bank 1 + if ((uint8_t)current_freq > (uint8_t)SPI_250KHZ) { m_spi_speed(current_freq); } +} + +void icm20948_set_gyro_range(m2_gpio_t cs_pin, g_range_t gyro_range) +{ + + spi_freq_t current_freq = _spi_freq; // Slow down SPI as needed + if ((uint8_t)current_freq > (uint8_t)SPI_1MHZ) { m_spi_speed(SPI_1MHZ); } + m_spi_write_register(cs_pin, REG_BANK_SEL, BANK_2); // switch to bank 2 + m_spi_write_register(cs_pin, GYRO_CONFIG, gyro_range); + m_spi_write_register(cs_pin, REG_BANK_SEL, BANK_0); // switch to bank 1 + if ((uint8_t)current_freq > (uint8_t)SPI_1MHZ) { m_spi_speed(current_freq); } +} + +void icm20948_get_raw_3dof(m2_gpio_t cs_pin, int16_t *buffer) { + buffer[0] = (256 * m_spi_read_register(cs_pin, ACCEL_XOUT_H) + + m_spi_read_register(cs_pin, ACCEL_XOUT_L)); + buffer[1] = (256 * m_spi_read_register(cs_pin, ACCEL_YOUT_H) + + m_spi_read_register(cs_pin, ACCEL_YOUT_L)); + buffer[2] = (256 * m_spi_read_register(cs_pin, GYRO_ZOUT_H) + + m_spi_read_register(cs_pin, GYRO_ZOUT_L)); +} + + +void icm20948_get_raw_6dof(m2_gpio_t cs_pin, int16_t *buffer_acc, int16_t *buffer_gyro) { + buffer_acc[0] = (256 * m_spi_read_register(cs_pin, ACCEL_XOUT_H) + m_spi_read_register(cs_pin, ACCEL_XOUT_L)); + buffer_acc[1] = (256 * m_spi_read_register(cs_pin, ACCEL_YOUT_H) + m_spi_read_register(cs_pin, ACCEL_YOUT_L)); + buffer_acc[2] = (256 * m_spi_read_register(cs_pin, ACCEL_ZOUT_H) + m_spi_read_register(cs_pin, ACCEL_ZOUT_L)); + buffer_gyro[0] = (256 * m_spi_read_register(cs_pin, GYRO_XOUT_H) + m_spi_read_register(cs_pin, GYRO_XOUT_L)); + buffer_gyro[1] = (256 * m_spi_read_register(cs_pin, GYRO_YOUT_H) + m_spi_read_register(cs_pin, GYRO_YOUT_L)); + buffer_gyro[2] = (256 * m_spi_read_register(cs_pin, GYRO_ZOUT_H) + m_spi_read_register(cs_pin, GYRO_ZOUT_L)); +} + + +void icm20948_calib_accel(m2_gpio_t *cs_pin, volatile int16_t *accel_adjust, int16_t *acc_ref0) { + for (int k = 0; k < 1; k++) { // loop over one IMU + for (int j = 0; j < NUM_DIM; j++) { // loop over all directions (x,y) + float accel_calib = 0; + for (int i = 0; i < CALIB_NUM_SAMPLES; i++) { + accel_calib += ((float)((m_spi_read_register(*(cs_pin + k), ACCEL_XOUT_H + j * 2) << 8) | + (m_spi_read_register(*(cs_pin + k), ACCEL_XOUT_L + j * 2))) / + (float)CALIB_NUM_SAMPLES); + // IMUS send out 16bit messages split in 2 bytes, ACCEL_XOUt_H: Adress to bits 8-15, ACCEL_XOUt_L: Address to bits 0-7 + } + (*(accel_adjust + k * 2 + j)) = (uint16_t)accel_calib - acc_ref0[j]; + } + } +} //! \end of mpu9250_calib_accel function \ No newline at end of file diff --git a/firmware/M2-on-wheelbot/src/m_bus.c b/firmware/M2-on-wheelbot/src/m_bus.c new file mode 100644 index 0000000..e5591e6 --- /dev/null +++ b/firmware/M2-on-wheelbot/src/m_bus.c @@ -0,0 +1,116 @@ +// ----------------------------------------------------------------------------- +// M2 data bus subsystem +// version: 2.0 +// date: May 30, 2011 +// author: J. Fiene +// ----------------------------------------------------------------------------- + +#include "m_bus.h" + +// private function prototypes: +unsigned char twi_start(unsigned char address, unsigned char readwrite); +unsigned char twi_send_byte(unsigned char byte); +unsigned char twi_read_byte(void); +void twi_stop(void); +unsigned char twi_wait_for_ack(void); + + +// PUBLIC FUNCTIONS +void m_bus_init(void) +{ + // ENABLE PULLUPS + set(PORTD,0); + set(PORTD,1); + set(PORTD,2); + + // CONFIGURE THE CLOCK + TWBR = 12; // CLK freq = CPU clock / (16 + 2*TWBR*(4^TWPS)), 16MHz clock, TWBR=12, TWPS=00 -> 400kHz + + // ENABLE interrupts in INT2 (D2) + set(EICRA,ISC21); clear(EICRA,ISC20); // trigger on falling edge + set(EIMSK,INT2); // demask the interrupt + sei(); // enable global interrupts +} + +unsigned char m_read_register(unsigned char addr, unsigned char reg) +{ + if(!twi_start(addr,WRITE)) return 0; // START + W + if(!twi_send_byte(reg)) return 0; // register to read + twi_stop(); // STOP + if(!twi_start(addr,READ)) return 0; // START + R + return(twi_read_byte()); // return register value +} + +unsigned char m_write_register(unsigned char addr, unsigned char reg, unsigned char value) +{ + if(!twi_start(addr,WRITE)) return 0; // START + W + if(!twi_send_byte(reg)) return 0; // register to write to + if(!twi_send_byte(value)) return 0; // value + twi_stop(); // STOP + return(1); +} + +// PRIVATE FUNCTIONS: + +// TWI: send START condition, wait for ACK, send ADDRESS with R/W flag +// readwrite = 1 for read, 0 for write +unsigned char twi_start(unsigned char address, unsigned char readwrite) +{ + unsigned char status; + + // START packet: + TWCR = (1< + + int + main () + { + unsigned int i, j; + unsigned int c; + int table[256]; + + for (i = 0; i < 256; i++) + { + for (c = i << 24, j = 8; j > 0; --j) + c = c & 0x80000000 ? (c << 1) ^ 0x04c11db7 : (c << 1); + table[i] = c; + } + + printf ("static const unsigned int crc32_table[] =\n{\n"); + for (i = 0; i < 256; i += 4) + { + printf (" 0x%08x, 0x%08x, 0x%08x, 0x%08x", + table[i + 0], table[i + 1], table[i + 2], table[i + 3]); + if (i + 4 < 256) + putchar (','); + putchar ('\n'); + } + printf ("};\n"); + return 0; + } + + For more information on CRC, see, e.g., + http://www.ross.net/crc/download/crc_v3.txt. */ + +#include "m_crc32.h" + +static const uint32_t crc32_table[] = +{ + 0x00000000, 0x04c11db7, 0x09823b6e, 0x0d4326d9, + 0x130476dc, 0x17c56b6b, 0x1a864db2, 0x1e475005, + 0x2608edb8, 0x22c9f00f, 0x2f8ad6d6, 0x2b4bcb61, + 0x350c9b64, 0x31cd86d3, 0x3c8ea00a, 0x384fbdbd, + 0x4c11db70, 0x48d0c6c7, 0x4593e01e, 0x4152fda9, + 0x5f15adac, 0x5bd4b01b, 0x569796c2, 0x52568b75, + 0x6a1936c8, 0x6ed82b7f, 0x639b0da6, 0x675a1011, + 0x791d4014, 0x7ddc5da3, 0x709f7b7a, 0x745e66cd, + 0x9823b6e0, 0x9ce2ab57, 0x91a18d8e, 0x95609039, + 0x8b27c03c, 0x8fe6dd8b, 0x82a5fb52, 0x8664e6e5, + 0xbe2b5b58, 0xbaea46ef, 0xb7a96036, 0xb3687d81, + 0xad2f2d84, 0xa9ee3033, 0xa4ad16ea, 0xa06c0b5d, + 0xd4326d90, 0xd0f37027, 0xddb056fe, 0xd9714b49, + 0xc7361b4c, 0xc3f706fb, 0xceb42022, 0xca753d95, + 0xf23a8028, 0xf6fb9d9f, 0xfbb8bb46, 0xff79a6f1, + 0xe13ef6f4, 0xe5ffeb43, 0xe8bccd9a, 0xec7dd02d, + 0x34867077, 0x30476dc0, 0x3d044b19, 0x39c556ae, + 0x278206ab, 0x23431b1c, 0x2e003dc5, 0x2ac12072, + 0x128e9dcf, 0x164f8078, 0x1b0ca6a1, 0x1fcdbb16, + 0x018aeb13, 0x054bf6a4, 0x0808d07d, 0x0cc9cdca, + 0x7897ab07, 0x7c56b6b0, 0x71159069, 0x75d48dde, + 0x6b93dddb, 0x6f52c06c, 0x6211e6b5, 0x66d0fb02, + 0x5e9f46bf, 0x5a5e5b08, 0x571d7dd1, 0x53dc6066, + 0x4d9b3063, 0x495a2dd4, 0x44190b0d, 0x40d816ba, + 0xaca5c697, 0xa864db20, 0xa527fdf9, 0xa1e6e04e, + 0xbfa1b04b, 0xbb60adfc, 0xb6238b25, 0xb2e29692, + 0x8aad2b2f, 0x8e6c3698, 0x832f1041, 0x87ee0df6, + 0x99a95df3, 0x9d684044, 0x902b669d, 0x94ea7b2a, + 0xe0b41de7, 0xe4750050, 0xe9362689, 0xedf73b3e, + 0xf3b06b3b, 0xf771768c, 0xfa325055, 0xfef34de2, + 0xc6bcf05f, 0xc27dede8, 0xcf3ecb31, 0xcbffd686, + 0xd5b88683, 0xd1799b34, 0xdc3abded, 0xd8fba05a, + 0x690ce0ee, 0x6dcdfd59, 0x608edb80, 0x644fc637, + 0x7a089632, 0x7ec98b85, 0x738aad5c, 0x774bb0eb, + 0x4f040d56, 0x4bc510e1, 0x46863638, 0x42472b8f, + 0x5c007b8a, 0x58c1663d, 0x558240e4, 0x51435d53, + 0x251d3b9e, 0x21dc2629, 0x2c9f00f0, 0x285e1d47, + 0x36194d42, 0x32d850f5, 0x3f9b762c, 0x3b5a6b9b, + 0x0315d626, 0x07d4cb91, 0x0a97ed48, 0x0e56f0ff, + 0x1011a0fa, 0x14d0bd4d, 0x19939b94, 0x1d528623, + 0xf12f560e, 0xf5ee4bb9, 0xf8ad6d60, 0xfc6c70d7, + 0xe22b20d2, 0xe6ea3d65, 0xeba91bbc, 0xef68060b, + 0xd727bbb6, 0xd3e6a601, 0xdea580d8, 0xda649d6f, + 0xc423cd6a, 0xc0e2d0dd, 0xcda1f604, 0xc960ebb3, + 0xbd3e8d7e, 0xb9ff90c9, 0xb4bcb610, 0xb07daba7, + 0xae3afba2, 0xaafbe615, 0xa7b8c0cc, 0xa379dd7b, + 0x9b3660c6, 0x9ff77d71, 0x92b45ba8, 0x9675461f, + 0x8832161a, 0x8cf30bad, 0x81b02d74, 0x857130c3, + 0x5d8a9099, 0x594b8d2e, 0x5408abf7, 0x50c9b640, + 0x4e8ee645, 0x4a4ffbf2, 0x470cdd2b, 0x43cdc09c, + 0x7b827d21, 0x7f436096, 0x7200464f, 0x76c15bf8, + 0x68860bfd, 0x6c47164a, 0x61043093, 0x65c52d24, + 0x119b4be9, 0x155a565e, 0x18197087, 0x1cd86d30, + 0x029f3d35, 0x065e2082, 0x0b1d065b, 0x0fdc1bec, + 0x3793a651, 0x3352bbe6, 0x3e119d3f, 0x3ad08088, + 0x2497d08d, 0x2056cd3a, 0x2d15ebe3, 0x29d4f654, + 0xc5a92679, 0xc1683bce, 0xcc2b1d17, 0xc8ea00a0, + 0xd6ad50a5, 0xd26c4d12, 0xdf2f6bcb, 0xdbee767c, + 0xe3a1cbc1, 0xe760d676, 0xea23f0af, 0xeee2ed18, + 0xf0a5bd1d, 0xf464a0aa, 0xf9278673, 0xfde69bc4, + 0x89b8fd09, 0x8d79e0be, 0x803ac667, 0x84fbdbd0, + 0x9abc8bd5, 0x9e7d9662, 0x933eb0bb, 0x97ffad0c, + 0xafb010b1, 0xab710d06, 0xa6322bdf, 0xa2f33668, + 0xbcb4666d, 0xb8757bda, 0xb5365d03, 0xb1f740b4 +}; + +/* + +@deftypefn Extension {unsigned int} crc32 (const unsigned char *@var{buf}, @ + int @var{len}, unsigned int @var{init}) + +Compute the 32-bit CRC of @var{buf} which has length @var{len}. The +starting value is @var{init}; this may be used to compute the CRC of +data split across multiple buffers by passing the return value of each +call as the @var{init} parameter of the next. + +This is used by the @command{gdb} remote protocol for the @samp{qCRC} +command. In order to get the same results as gdb for a block of data, +you must pass the first CRC parameter as @code{0xffffffff}. + +This CRC can be specified as: + + Width : 32 + Poly : 0x04c11db7 + Init : parameter, typically 0xffffffff + RefIn : false + RefOut : false + XorOut : 0 + +This differs from the "standard" CRC-32 algorithm in that the values +are not reflected, and there is no final XOR value. These differences +make it easy to compose the values of multiple blocks. + +@end deftypefn + +*/ + +uint32_t m_crc32 (uint8_t *buffer, int length) +{ + uint32_t crc = 0xFFFFFFFF; + while (length--) + { + crc = (crc << 8) ^ crc32_table[((crc >> 24) ^ *buffer) & 255]; + buffer++; + } + return crc; +} diff --git a/firmware/M2-on-wheelbot/src/m_rf.c b/firmware/M2-on-wheelbot/src/m_rf.c new file mode 100644 index 0000000..d4e9c95 --- /dev/null +++ b/firmware/M2-on-wheelbot/src/m_rf.c @@ -0,0 +1,153 @@ +// ----------------------------------------------------------------------------- +// M2 RF communication subsystem +// version: 2.0 +// date: June 30, 2011 +// author: J. Fiene +// ----------------------------------------------------------------------------- + +#include "m_rf.h" + +#define MRFTWIADDR 0x28 +#define MRFINIT 0x01 +#define MRFREAD 0x02 +#define MRFSEND 0x03 + +char m_rf_open(char channel, char RXaddress, char packet_length) +{ + // START | MRFTWIADDR | MRFINIT | channel | RXaddress | packet_length | STOP + + m_bus_init(); + + // START + TWCR = (1<> 8) & 255) + +#define HW_CONFIG() (UHWCON = 0x01) + +#ifdef M1 +#define PLL_CONFIG() (PLLCSR = 0x02) // fixed to 8MHz clock +#else +#define PLL_CONFIG() (PLLCSR = 0x12) // 0001 0010 For a 16MHz clock +#endif + +#define USB_CONFIG() (USBCON = ((1< size) write_size = size; + size -= write_size; + + // write the packet + switch (write_size) { + #if (CDC_TX_SIZE == 64) + case 64: UEDATX = *buffer++; + case 63: UEDATX = *buffer++; + case 62: UEDATX = *buffer++; + case 61: UEDATX = *buffer++; + case 60: UEDATX = *buffer++; + case 59: UEDATX = *buffer++; + case 58: UEDATX = *buffer++; + case 57: UEDATX = *buffer++; + case 56: UEDATX = *buffer++; + case 55: UEDATX = *buffer++; + case 54: UEDATX = *buffer++; + case 53: UEDATX = *buffer++; + case 52: UEDATX = *buffer++; + case 51: UEDATX = *buffer++; + case 50: UEDATX = *buffer++; + case 49: UEDATX = *buffer++; + case 48: UEDATX = *buffer++; + case 47: UEDATX = *buffer++; + case 46: UEDATX = *buffer++; + case 45: UEDATX = *buffer++; + case 44: UEDATX = *buffer++; + case 43: UEDATX = *buffer++; + case 42: UEDATX = *buffer++; + case 41: UEDATX = *buffer++; + case 40: UEDATX = *buffer++; + case 39: UEDATX = *buffer++; + case 38: UEDATX = *buffer++; + case 37: UEDATX = *buffer++; + case 36: UEDATX = *buffer++; + case 35: UEDATX = *buffer++; + case 34: UEDATX = *buffer++; + case 33: UEDATX = *buffer++; + #endif + #if (CDC_TX_SIZE >= 32) + case 32: UEDATX = *buffer++; + case 31: UEDATX = *buffer++; + case 30: UEDATX = *buffer++; + case 29: UEDATX = *buffer++; + case 28: UEDATX = *buffer++; + case 27: UEDATX = *buffer++; + case 26: UEDATX = *buffer++; + case 25: UEDATX = *buffer++; + case 24: UEDATX = *buffer++; + case 23: UEDATX = *buffer++; + case 22: UEDATX = *buffer++; + case 21: UEDATX = *buffer++; + case 20: UEDATX = *buffer++; + case 19: UEDATX = *buffer++; + case 18: UEDATX = *buffer++; + case 17: UEDATX = *buffer++; + #endif + #if (CDC_TX_SIZE >= 16) + case 16: UEDATX = *buffer++; + case 15: UEDATX = *buffer++; + case 14: UEDATX = *buffer++; + case 13: UEDATX = *buffer++; + case 12: UEDATX = *buffer++; + case 11: UEDATX = *buffer++; + case 10: UEDATX = *buffer++; + case 9: UEDATX = *buffer++; + #endif + case 8: UEDATX = *buffer++; + case 7: UEDATX = *buffer++; + case 6: UEDATX = *buffer++; + case 5: UEDATX = *buffer++; + case 4: UEDATX = *buffer++; + case 3: UEDATX = *buffer++; + case 2: UEDATX = *buffer++; + default: + case 1: UEDATX = *buffer++; + case 0: break; + } + // if this completed a packet, transmit it now! + if (!(UEINTX & (1<= NUM_DESC_LIST) { + UECONX = (1< desc_length) len = desc_length; + do { + // wait for host ready for IN packet + do { + i = UEINTX; + } while (!(i & ((1<= 1 && i <= MAX_ENDPOINT) { + usb_send_in(); + UENUM = i; + if (bRequest == SET_FEATURE) { + UECONX = (1<> 4); + phex1(c & 15); +} + +void m_usb_tx_hex(unsigned int i) +{ + phex(i >> 8); + phex(i); +} + +void m_usb_tx_hexchar(unsigned char i) +{ + phex(i); +} + +void m_usb_tx_int(int i) +{ + char string[7] = {0,0,0,0,0,0,0}; + itoa(i,string,10); + for(i=0;i<7;i++){ + if(string[i]){ + m_usb_tx_char(string[i]); + } + } +} + +void m_usb_tx_uint(unsigned int i) +{ + char string[6] = {0,0,0,0,0,0}; + utoa(i,string,10); + for(i=0;i<5;i++){ + if(string[i]){ + m_usb_tx_char(string[i]); + } + } +} + +void m_usb_tx_long(long i) +{ + char string[11] = {0,0,0,0,0,0,0,0,0,0,0}; + ltoa(i,string,10); + for(i=0;i<11;i++){ + if(string[i]){ + m_usb_tx_char(string[i]); + } + } +} + +void m_usb_tx_ulong(unsigned long i) +{ + char string[11] = {0,0,0,0,0,0,0,0,0,0,0}; + ultoa(i,string,10); + for(i=0;i<10;i++){ + if(string[i]){ + m_usb_tx_char(string[i]); + } + } +} diff --git a/firmware/M2-on-wheelbot/src/main.c b/firmware/M2-on-wheelbot/src/main.c new file mode 100644 index 0000000..d0aba4b --- /dev/null +++ b/firmware/M2-on-wheelbot/src/main.c @@ -0,0 +1,946 @@ +/************************************************************************** + * + * Header Files to include + * + **************************************************************************/ +/* subystem header files */ +#include "control.h" +#include "icm20948.h" +#include "m_bus.h" +#include "m_rf.h" +#include "m_spi.h" +#include "m_usb.h" +#include "matrix_calculations.h" +#include "state_estimation.h" +#include "udriver.h" +#include "checks.h" + +/* global program files */ +#include "config.h" +#include "m_general.h" + +/* External header files */ +#include // Watchdog +#include // Used in check_crc + +/************************************************************************** + * + * Function declarations + * + **************************************************************************/ +void init(); // prototype the initialization function +void setup_timer(); +uint16_t select_timer1_prescaler(); +uint16_t micros( bool stream ); +void init_calibrate_udriver(); +bool check_crc( uint8_t *data_in_ptr ); +void data_write( uint8_t *packet, int index, float scaling, float data ); +void read_usb( uint8_t *buffer ); + + /************************************************************************** + * + * Global Variables - these are the only non-stack RAM usage + * + **************************************************************************/ +volatile bool tick = FALSE; // ISR tick when loop is executed +volatile bool new_data = FALSE; // ISR tick when wifi data_out is being received +uint16_t counter = 0; // counter used to count control loops +uint16_t counter_lastwifi = 0; // Last loop that wifi signal has been received +volatile uint8_t cycle_overflow_counter = 0; +Commstruct comm_ud; // Change scope to main and why volatile?? + +bool stream = TRUE; // Start streaming input +bool transmitOK; + +/************************************************************************** + * + * Main function + * + **************************************************************************/ +int main() { + /* Error messages are stored in sys_flag + * 0: All good + * 1: Encoder 1 not properly initialized + * 2: Encoder 2 not properly initialized + * 3: Loop execution time is too long, check TICK (> 0.01 seconds) + * 4: Loop did not fully execute + * 5: Loop execution time is almost too long (>0.08 seconds) + * 6: Estimator calibration has been triggered + */ + uint16_t sys_flag = 0; + float max_rate_motor2 = 50.0; // Max rate in rad/s, 400 RPM + + /* + * usb_control: If TRUE, USB connection is used instead of RF + * use_IMUS: If FALSE, no IMU readings are obtained and control is switched off + */ + FlagsStruct flags = { .system = TRUE, .wifi = TRUE, .angle = TRUE, .rate = TRUE, .control = TRUE, .battery = TRUE, + .upright = FALSE, .standup1 = FALSE, .standup1_done = FALSE, .standup2 = FALSE, + .standup2_done = FALSE, .bias = FALSE , .loop_finished = TRUE, .LQRcontrol1 = FALSE, .LQRcontrol2 = FALSE, + .useLQR1 = FALSE, .useLQR2 = FALSE, .do_standup = FALSE, .do_rollup = FALSE, + .usb_control = FALSE, .use_IMUs = TRUE, .motor_test = FALSE, + .use_pivotAcc_CP = TRUE, .use_pivotAcc_WC_vel = FALSE, .use_pivotAcc_WC_acc = FALSE}; + + uint16_t battery_voltage; + uint16_t tick_time = 0; // Check time between ticks + uint16_t loop_time = 0; // Check time of control loop + + float time_test_start = 0.0; // Variable used in function "test_motor" to store start time of motor test + + //////////////////////////////////////// + // uDRIVER - MOTOR CONTROL VARIABLES + //////////////////////////////////////// + Commstruct comm_ud = { .currentLimit1=0.0, .currentTargetMotor1=0.0, .currentLimit2=0.0, .currentTargetMotor2=0.0, + .currentMotor1=0.0, .positionMotor1=0.0, .velocityMotor1=0.0, .currentMotor2=0.0, + .positionMotor2=0.0, .velocityMotor2=0.0, .status_errcode=0, .status_SE=FALSE, .status_M1E=FALSE, + .status_M1R=FALSE, .status_IDX1D=FALSE, .status_IDX1T=FALSE, .status_M2E=FALSE, .status_M2R=FALSE, + .status_IDX2D=FALSE, .status_IDX2T=FALSE }; + + //////////////////////////////////////// + // IMU VARIABLES + //////////////////////////////////////// + a_range_t acc_range = ACCEL_2G; + g_range_t gyro_range = GYRO_500DPS; //g_range_t gyro_range = GYRO_2000DPS; + float accel_scaling = 0.000598755; // ACCEL_2G transforms bit in m/s2 //float accel_scaling = 0.00119751; // ACCEL_4G + float gyro_scaling = 0.0002663161; // Transform bit to max 500°/s in rad/s + + m2_gpio_t imu_list[NUM_SENSORS] = {IMU_A, IMU_B, IMU_C, IMU_D}; // Chip select pins for each sensor + float IMUx[NUM_SENSORS] = {-PI/2, -PI/2, PI/2, PI/2}; + float IMUz[NUM_SENSORS] = {(-3*PI)/4, PI/4, -PI/4, (3*PI)/4}; + int32_t R_b_IMU[3][3] = {{0.0}}; + + int16_t accel_adjust[NUM_SENSORS][3] = {0}; // bias adjust for accel + int16_t imu_acc[3] = {0}; + //int16_t imu_acc_C[3] = {0}; + int32_t imu_acc_tmp[3] = {0}; + + int16_t imu_gyro[3] = {0}; + int32_t imu_gyro_tmp[3] = {0}; + + int16_t imu_data[NUM_SENSORS][NUM_IMU_DATA] = {0}; // data in the body frame + + float body_rate[NUM_DIM] = {0}; // Gyroscope body rate estimate in body frame + //float euler_rate[NUM_DIM] = {0}; // Gyroscope body rate estimate as Euler rates DELETE + + //////////////////////////////////////// + // VARIABLES FOR ESTIMATOR + //////////////////////////////////////// + float roll_bias = 0.07; //0.05; //0,0043 // -0.0124; //-0.02; //0.0; //0.00331; //0.00874; //-0.04; //-0.03; //-0.0416; //-0.005; //-0.01; //-0.048; //0.026; + float pitch_bias = 0.00;//0.017; //-0.0523; //−0,0863//0.0173; //-0.05; //-0.05; //-0.037; //-0.04; //-0.062; //-0.031; + + float roll_bias_sum = 0.0; + float pitch_bias_sum = 0.0; + + float q[5] = {INIT_ANGLE, 0.0, 0.0, 0.0, 0.0}; // Initial Roll and Pitch angle + float q_acc[2] = {INIT_ANGLE, 0.0}; // Attitude est. from accelerometer measurements, set to zero before calibration + float q_acc_nopivot[2] = {INIT_ANGLE, 0.0}; // DEBUG - DELETE + float dq[5] = {INIT_ANGLE, 0.0, 0.0, 0.0, 0.0}; // Initial Roll and Pitch angle + float dq4_prev = 0.0; + + float ddq4_est = 0.0; + float acc_pivot_B[3] = {0.0, 0.0, 0.0}; + float acc_pivot_B0[3] = {0.0, 0.0, 0.0}; // DEBUG - DELETE + float T[3][3] = {0}; // Transformation matrix from body rate to euler rate + T[0][0] = cos(q[1]); + T[0][1] = 0.0; + T[0][2] = sin(q[1]); + T[1][0] = 0.0; + T[1][1] = 1; + T[1][2] = 0; + T[2][0] = -cos(q[0])*sin(q[1]); + T[2][1] = sin(q[0]); + T[2][2] = cos(q[0])*cos(q[1]); + + int16_t acc_ref0[NUM_SENSORS][NUM_DIM] = {0}; // Array to store reference acceleration in IMU frames to get bias in calibration + uint16_t counter_tmp = 0; + uint16_t counts = 0; + counter_ptr = count_start; + + //////////////////////////////////////// + // VARIABLES FOR WIFI + //////////////////////////////////////// + uint8_t data_in[PACKET_LENGTH_IN] = {0}; // data coming from PC + uint8_t data_in_prev[PACKET_LENGTH_IN] = {0}; // data coming from PC + uint8_t data_out[PACKET_LENGTH] = {0}; // data sent to PC + uint8_t data_in0 = 0; + + uint32_t *crc_out_ptr = (uint32_t *)&data_out[28]; + bool crc_in_OK = TRUE; + + float scaling_attitude = 32767.0 / PI; + float scaling_current = 32767.0 / 40; + float scaling_rate = (32767.0 / 3000) * (60 / (2 * PI)); // Motorrate is measured in rad/s, max rate 3000 RPM + float init_q5_est = 0.0; + float init_q4_est = 0.0; + + uint16_t durations[5] = {0}; + float controls[5] = {0.0}; + float* motor_ptr; + bool* flag_ptr; + uint16_t ramp_length1 = 0; + uint16_t ramp_length2 = 5; + + + // *************************************** + // ININITILIZATION + // *************************************** + init(); + + // UDRIVER INIT + init_calibrate_udriver(&comm_ud); // initialize and calibrate BLDC motor + + if ( flags.use_IMUs == TRUE ){ + + start1: // initialize each IMU with same settings + for ( int i = 0; i < NUM_SENSORS; ++i ) { + if ( icm20948_init( imu_list[i], acc_range, gyro_range ) ) { + m_green( ON ); + m_wait( INIT_WAIT ); + m_green( OFF ); + } else { + m_red( TOGGLE ); + m_wait( 5 * INIT_WAIT ); + goto start1; + } + } // end for + + // SPI speed reset, somehow IMU init. takes different SPI_MHZ (?) + m_spi_speed(SPI_4MHZ); + //m_spi_speed(SPI_1MHZ); + //m_spi_speed(SPI_500KHZ); + //m_spi_speed( SPI_125KHZ ); + + // measure accelerometer bias. can be stored in hardware for the future + acc_ref0[0][0] = 5793; // x, initial acceleration in IMU frame taken from simulation + acc_ref0[0][1] = -14189; // y, Note that 1g = 16384 + acc_ref0[0][2] = 5793; // z + + acc_ref0[1][0] = -5793; // x + acc_ref0[1][1] = -14189; // y + acc_ref0[1][2] = -5793; // z + + acc_ref0[2][0] = 5793; // x + acc_ref0[2][1] = 14189; // y + acc_ref0[2][2] = 5793; // z + + acc_ref0[3][0] = -5793; // x + acc_ref0[3][1] = 14189; // y + acc_ref0[3][2] = -5793; // z + + icm20948_calib_accel(&imu_list[0], &accel_adjust[0][0], acc_ref0[0]); + icm20948_calib_accel(&imu_list[1], &accel_adjust[1][0], acc_ref0[1]); + icm20948_calib_accel(&imu_list[2], &accel_adjust[2][0], acc_ref0[2]); + icm20948_calib_accel(&imu_list[3], &accel_adjust[3][0], acc_ref0[3]); + } // end if + + start2: // wait until python interface sends "Start wheelbot" signal + if (data_in[0] == 'i') { + m_green( ON ); + } else { + + if (flags.usb_control == TRUE) { + if( m_usb_rx_available() ) + { + read_usb( &data_in[0] ); + } + data_out[PACKET_LENGTH-2] = 0; + data_out[PACKET_LENGTH-1] = 0; // If !crc_2PC_OK set 'crc2' to 1 + m_usb_tx_char('X'); + usb_serial_write(data_out, PACKET_LENGTH); + } + else + { + if ( new_data == TRUE ) + { + new_data = FALSE; + m_rf_read( (char *) data_in, PACKET_LENGTH_IN ); + check_crc( data_in ); + } + *crc_out_ptr = m_crc32( (uint8_t *)data_out, 28 ); + m_rf_send( TXADDRESS, (char *) data_out, PACKET_LENGTH ); + } + + m_green( TOGGLE ); + m_red( TOGGLE ); + m_wait( INIT_WAIT ); + goto start2; + } + + tick_time = micros(FALSE); + m_red( OFF ); // phew, we made it + m_green( OFF ); + + // ****************************************************************************** + // + // WHILE-LOOP + // + // ****************************************************************************** + + while (1) { + + if (tick) { + tick_time = micros(FALSE); + ++counter; + tick = FALSE; + check_tick_time(&flags, &sys_flag, &tick_time); + + + // *************************************** + // GET MOTOR RATES + // *************************************** + dq[3] = 0.3 * comm_ud.velocityMotor2 + 0.7 * dq[3]; // LOWER WHEEL + dq[4] = 0.3 * comm_ud.velocityMotor1 + 0.7 * dq[4]; // UPPER WHEEL + + q[3] += (dq[3] / (float)LOOP_FREQ); // Integrate rates + q[4] += (dq[4] / (float)LOOP_FREQ); + + //ddq4_est = 0.9 * (dq[3] - dq4_prev) * LOOP_FREQ + 0.1 * ddq4_est; // LP filter derivative of dq[3] + //ddq4_est = (dq[3] - dq4_prev) * LOOP_FREQ; // Derivative of dq[3] + //dq4_prev = dq[3]; + //ddq4_est = 0.8 * (comm_ud.velocityMotor2 - dq4_prev) * LOOP_FREQ + 0.2 * ddq4_est; // LP filter derivative of dq_4 + ddq4_est = (comm_ud.velocityMotor2 - dq4_prev) * LOOP_FREQ; // Derivative of dq_4 + dq4_prev = comm_ud.velocityMotor2; + + if ( flags.use_IMUs == TRUE ) + { + + // *************************************** + // GET IMU data, REMOVE ACC BIAS, TRANSFORM TO BODY-FIXED FRAME + // *************************************** + for (int i = 0; i < NUM_SENSORS; ++i) { + icm20948_get_raw_6dof(imu_list[i], imu_acc, imu_gyro); + + for (int j = 0; j < 3; ++j) { + imu_acc[j] -= accel_adjust[i][j]; + } + + rot_b_IMU(R_b_IMU, IMUx[i], IMUz[i]); // Compute body-frame transform scaled by 10000 + multiply_matrix_vector_long(3, 3, R_b_IMU, imu_acc, imu_acc_tmp); + multiply_matrix_vector_long(3, 3, R_b_IMU, imu_gyro, imu_gyro_tmp); + + for (int j = 0; j < 3; ++j) { + imu_data[i][j] = (int)( imu_acc_tmp[j] / 10000 ); // the entries of rot_b_IMU are scaled by 10000 + imu_data[i][j+3] = (int)( imu_gyro_tmp[j] / 10000 ); + } + } + + body_rate[0] = + (float)( imu_data[0][3] * 0.25 + imu_data[1][3] * 0.25 + imu_data[2][3] * 0.25 + imu_data[3][3] * 0.25 ) * gyro_scaling; // x + body_rate[1] = + (float)( imu_data[0][4] * 0.25 + imu_data[1][4] * 0.25 + imu_data[2][4] * 0.25 + imu_data[3][4] * 0.25 ) * gyro_scaling; // y + body_rate[2] = + (float)( imu_data[0][5] * 0.25 + imu_data[1][5] * 0.25 + imu_data[2][5] * 0.25 + imu_data[3][5] * 0.25) * gyro_scaling; // z + + + // *************************************** + // TRANSFORM BODY RATE TO EULER RATES + // *************************************** + // Transformation matrix from body rate to Euler rate + // Note: Euler rate defined here as [Roll, Pitch, Yaw] + T[0][0] = cos(q[1]); + T[0][1] = 0.0; + T[0][2] = sin(q[1]); + T[1][0] = 0.0; + T[1][1] = 1; + T[1][2] = 0; + T[2][0] = -cos(q[0])*sin(q[1]); + T[2][1] = sin(q[0]); + T[2][2] = cos(q[0])*cos(q[1]); + + multiply_matrix_vector(3, 3, T, body_rate, dq); + + + // *************************************** + // GET ATTITUDE ESTIMATE FROM GYRO + // *************************************** + q[0] += (dq[0] / LOOP_FREQ); + q[1] += (dq[1] / LOOP_FREQ); + + + // *************************************** + // ESTIMATE STATE + // *************************************** + estimate_pivot_acc_linearized(acc_pivot_B, q, dq, ddq4_est, &flags); // Compute acc_pivot_B + attitude_estimate_accel(q_acc, acc_pivot_B, &imu_data[0][0]); // Compute q_acc + + //attitude_estimate_accel(q_acc_nopivot, acc_pivot_B0, &imu_data[0][0]); + + q[0] = FUSION_TUNING_PARAMETER * q_acc[0] + (1 - FUSION_TUNING_PARAMETER) * q[0] ; + q[1] = FUSION_TUNING_PARAMETER * q_acc[1] + (1 - FUSION_TUNING_PARAMETER) * q[1] ; + + } // end if + + + // *************************************** + // EXECUTE COMMAND e.g. compute control + // *************************************** + + // LQR CONTROL + if ( data_in[0] == 'C' ) + { + flags.useLQR1 = TRUE; + flags.useLQR2 = TRUE; + } + // Two-step standup + else if ( data_in[0] == 'S' || data_in[0] == 's') + { + flags.do_standup = TRUE; + motor_ptr = &comm_ud.currentTargetMotor1; + + if ( data_in[0] == 's' ) { flags.standup1_done = TRUE; } + + if ( !flags.standup1_done && ( fabs(q[0]) > 0.072 ) && (comm_ud.velocityMotor1 < 220) ) + { + flag_ptr = &flags.standup1_done; + durations[0] = 1; durations[1] = 100; durations[2] =120; durations[3] = 150; durations[4] = 0; + controls[0] = 0.0; controls[1] = -5.0; controls[2] = -0.0; controls[3] = 15.56; controls[4] = 0; + } + else if ( !flags.standup2_done ) + { + if (flags.standup2 = FALSE) + { + flags.standup2 = TRUE; + flags.standup1_done = TRUE; + counter_ptr = count_start; // Resets count function + counts = 0; // Resets counts just to be sure + } + ramp_length2 = 10; + flag_ptr = &flags.standup2_done; + durations[0] = 150; durations[1] = 250; durations[2] = 310; durations[3] = 340; durations[4] = 0; + controls[0] = 0.0; controls[1] = -5.0; controls[2] = -0.5; controls[3] = 14.53; controls[4] = 0; + + if (q[0] < 0.1 && counts > 250) { flags.standup2_done = TRUE; flags.useLQR1 = TRUE; } + if (fabs(q[0]) < 0.3 && counts > 250) { flags.useLQR2 = TRUE; } + } + else + { + flags.do_standup = FALSE; + flags.standup2_done = TRUE; + flags.useLQR1 = TRUE; + flags.useLQR2 = TRUE; + } + } + // Two-step roll-up + else if ( data_in[0] == 'U' || data_in[0] == 'u') + { + flags.do_standup = TRUE; + motor_ptr = &comm_ud.currentTargetMotor2; + + if ( data_in[0] == 'u' ) { flags.standup1_done = TRUE; } + + if ( !flags.standup1_done || comm_ud.velocityMotor2 > 270 ) + { + ramp_length2 = 10; + max_rate_motor2 = MAX_RATE_MOTOR1; + flag_ptr = &flags.standup1_done; + durations[0] = 1; durations[1] = 100; durations[2] = 120; durations[3] =155; durations[4] = 195; + controls[0] = 0.0; controls[1] = -5.0, controls[2] = 0.0; controls[3] = 14.93; controls[4] = -5.6; + } + else if ( !flags.standup2_done ) + { + if (flags.standup2 = FALSE) + { + flags.standup2 = TRUE; + flags.standup1_done = TRUE; + counter_ptr = count_start; // Resets count function + counts = 0; // Resets counts just to be sure + max_rate_motor2 = 50; + } + ramp_length2 = 4; + flag_ptr = &flags.standup2_done; + durations[0] = 1; durations[1] = 2; durations[2] = 3; durations[3] = 350; durations[4] = 360; + controls[0] = 0.0; controls[2] = 0.0; controls[2] = 0.0; controls[3] = 0.0; durations[4] = 6.0; + + if (fabs(q[1]) < 0.3 && counts > 250) { flags.useLQR1 = TRUE; } + if (fabs(q[1]) < 0.05 && counts > 250) { flags.standup2_done = TRUE; flags.useLQR2 = TRUE; } + } + else + { + max_rate_motor2 = 50; + flags.do_standup = FALSE; + flags.standup2_done = TRUE; + flags.useLQR1 = TRUE; + flags.useLQR2 = TRUE; + } + } + // CALIBRATE ESTIMATOR + else if (data_in[0] == 'A') + { + sys_flag = 6; + flags.useLQR1 = TRUE; + flags.useLQR2 = TRUE; + + // Estimate bias + compute_estimator_bias(&flags, counter, &counter_tmp, &roll_bias, &roll_bias_sum, + &pitch_bias, &pitch_bias_sum, q ); + } + // SIMPLE MOTOR COMMANDS + else if (data_in[0] == 'M') + { + comm_ud.currentTargetMotor1 = 1.0; + comm_ud.currentTargetMotor2 = 0.0; + } + else if (data_in[0] == 'm') { + comm_ud.currentTargetMotor1 = -1.0; + comm_ud.currentTargetMotor2 = 0.0; + } + else if (data_in[0] == 'T') { + comm_ud.currentTargetMotor1 = 0.0; + comm_ud.currentTargetMotor2 = 1.0; + } + else if (data_in[0] == 't') { + comm_ud.currentTargetMotor1 = 0.0; + comm_ud.currentTargetMotor2 = -1.0; + } + else if (data_in[0] == 'K') { + comm_ud.currentTargetMotor1 = 1.5 * test_motor(&flags, counter, &time_test_start); // TEST MOTOR CONTROL + } + + // JUMP + if ( flags.do_standup ) { feedforward_signal(motor_ptr, flag_ptr, &counter, &counter_tmp, &counts, durations, controls, ramp_length1, ramp_length2); } + + // ROLL CONTROLLER + if (flags.useLQR1) + { + if (flags.LQRcontrol1 == FALSE) { flags.LQRcontrol1 = TRUE; init_q5_est = q[4]; } + comm_ud.currentTargetMotor1 = return_current_roll(q[0]-roll_bias, dq[0], q[4] - init_q5_est, dq[4]); + } + + // PITCH CONTROLLER + if (flags.useLQR2) + { + if (flags.LQRcontrol2 == FALSE) { flags.LQRcontrol2 = TRUE; init_q4_est = q[3]; } + comm_ud.currentTargetMotor2 = return_current_pitch(q[1]-pitch_bias, dq[1], q[3] - init_q4_est, dq[3]); + } + + // *************************************** + // CHECK SAFETY AND SEND CONTROL + // *************************************** + //reset_checks(&flags, &comm_ud, &data_in[0]); + check_control(&flags, &comm_ud); + check_rates(&flags, &comm_ud, max_rate_motor2); + //check_battery(&flags, &battery_voltage); + check_upright(&flags, &comm_ud, q); + + if ( flags.usb_control ){ check_wifi(&flags, counter, counter_lastwifi); } + + flags.system = flags.system && flags.wifi && flags.angle && flags.rate && flags.control && flags.battery; + + if (!flags.system || data_in[0] == 'e') + { + comm_ud.currentTargetMotor1 = 0.0; + comm_ud.currentTargetMotor2 = 0.0; + send_spi_udriver(&comm_ud); + m_red( ON ); + } + else if (flags.system) + { + send_spi_udriver(&comm_ud); // Send current to motor + } + + + // *************************************** + // USB / WIFI COMMUNICATION + // *************************************** + #if DATA_MODE == 1 // GET DATA FOR CONTROL PLOTS + data_write(data_out, 0, scaling_attitude, q[0] - roll_bias); // roll, q[0] - roll_bias, q_acc[0] + data_write(data_out, 2, scaling_attitude, q[1] - pitch_bias); // pitch, q[1] - pitch_bias, q_acc[1] + data_write(data_out, 4, 1/gyro_scaling, dq[0]); // roll_rate + data_write(data_out, 6, 1/gyro_scaling, dq[1]); // pitch_rate + data_write(data_out, 8, scaling_current, comm_ud.currentTargetMotor1); // motor 1 current sent + data_write(data_out, 10, scaling_current, comm_ud.currentTargetMotor2); // motor 2 current sent + data_write(data_out, 12, scaling_rate, dq[4]); // motor 1 rate - comm_ud.velocityMotor1 + data_write(data_out, 14, scaling_rate, dq[3]); // motor 2 rate - comm_ud.velocityMotor2 + + #if FLAG_PLOT == 1 // MISC + data_write(data_out, 16, scaling_current, comm_ud.currentTargetMotor2); // DEBUG 1 + data_write(data_out, 18, 32767/200.0, acc_pivot_B[0]); // DEBUG 2 + data_write(data_out, 20, scaling_rate, dq4_prev); // DEBUG 3 + #endif + + #if FLAG_PLOT == 2 // SYS FLAG + MOTOR POS + data_write(data_out, 16, 1.0, sys_flag); // DEBUG 1 + data_write(data_out, 18, 32767.0 / (10000*PI), q[4] - init_q5_est); // DEBUG 2 + data_write(data_out, 20, 32767.0 / (10000*PI), q[3] - init_q4_est); // DEBUG 3 + #endif + + #if FLAG_PLOT == 3 + data_write(data_out, 16, 1.0, sys_flag); // DEBUG 1 + data_write(data_out, 18, 0.5, loop_time); // DEBUG 2 + data_write(data_out, 20, 0.5, tick_time); // DEBUG 3 + #endif + + #if FLAG_PLOT == 6 // PIVOT ACC + data_write(data_out, 16, 32767/200.0, acc_pivot_B[0]); // DEBUG 1 + data_write(data_out, 18, scaling_rate, comm_ud.velocityMotor1); // DEBUG 2 + data_write(data_out, 20, scaling_current, comm_ud.currentTargetMotor2); // DEBUG 3 + #endif + + #if FLAG_PLOT == 7 // ESTIMATOR BIAS + data_write(data_out, 16, 1.0, sys_flag); // DEBUG 1 + data_write(data_out, 18, 32767/1.8, roll_bias); // DEBUG 2 + data_write(data_out, 20, 32767/1.8, pitch_bias); // DEBUG 3 + #endif + + #if FLAG_PLOT == 8 // DATA IN + data_write(data_out, 16, 1.0, data_in[0]); // DEBUG 1 + data_write(data_out, 18, 1.0, data_in[1]); // DEBUG 2 + data_write(data_out, 20, 1.0, data_in[9]); // DEBUG 3 + #endif + + #if FLAG_PLOT == 9 // DATA IN + data_write(data_out, 16, 1.0, sys_flag); // DEBUG 1 + data_write(data_out, 18, scaling_current, comm_ud.currentMotor1); // DEBUG 2 + data_write(data_out, 20, scaling_current, comm_ud.currentMotor2); // DEBUG 3 + #endif + + #if FLAG_PLOT == 10 // DATA IN + data_write(data_out, 16, 1/gyro_scaling, dq[2]); // DEBUG 1 + data_write(data_out, 18, scaling_attitude, q_acc[0]); // DEBUG 2 + data_write(data_out, 20, scaling_attitude, q_acc[1]); // DEBUG 3 + #endif + + #if FLAG_PLOT == 11 // DATA IN + data_write(data_out, 16, 1/accel_scaling, acc_pivot_B[0]); // DEBUG 1 + data_write(data_out, 18, 1/accel_scaling, acc_pivot_B[1]); // DEBUG 2 + data_write(data_out, 20, 1/accel_scaling, acc_pivot_B[2]); // DEBUG 3 + #endif + + #if FLAG_PLOT == 12 // DATA IN + data_write(data_out, 16, scaling_attitude, q_acc_nopivot[0]); // DEBUG 1 + data_write(data_out, 18, scaling_attitude, q_acc_nopivot[1]); // DEBUG 2 + data_write(data_out, 20, scaling_attitude, q_acc_nopivot[2]); // DEBUG 3 + #endif + + #if FLAG_PLOT == 13 // DATA IN + data_write(data_out, 16, 1, imu_data[3][0]); // DEBUG 1 + data_write(data_out, 18, 1/accel_scaling, acc_pivot_B[0]); // DEBUG 2 + data_write(data_out, 20, scaling_rate, comm_ud.velocityMotor2); // DEBUG 3 + #endif + #endif //END DATA_MODE == 1 + + #if DATA_MODE == 2 // ANALYSE PIVOT ACC ESTIMATOR + data_write(data_out, 0, scaling_attitude, q_acc[0] ); + data_write(data_out, 2, scaling_attitude, q_acc[1] ); + data_write(data_out, 4, scaling_attitude, q_acc_nopivot[0] ); + data_write(data_out, 6, scaling_attitude, q_acc_nopivot[1] ); + data_write(data_out, 8, scaling_rate, dq[3]); + data_write(data_out, 10, scaling_rate, comm_ud.velocityMotor2); + data_write(data_out, 12, 1, imu_data[2][0]); + data_write(data_out, 14, 1, imu_data[3][0]); + data_write(data_out, 16, 1, imu_data[2][1]); + data_write(data_out, 18, 1/accel_scaling, acc_pivot_B[0]); + data_write(data_out, 20, 1/accel_scaling, acc_pivot_B[1]); + #endif + + #if DATA_MODE == 3 // ANALYSE IMU ACC VALUES + data_write(data_out, 0, 1, imu_data[0][0]); + data_write(data_out, 2, 1, imu_data[0][1]); + data_write(data_out, 4, 1, imu_data[0][2]); + data_write(data_out, 6, 1, imu_data[1][0]); + data_write(data_out, 8, 1, imu_data[1][1]); + data_write(data_out, 10, 1, imu_data[1][2]); + data_write(data_out, 12, 1, imu_data[2][0]); + data_write(data_out, 14, 1, imu_data[2][1]); + data_write(data_out, 16, 1, imu_data[2][2]); + data_write(data_out, 18, 1, imu_data[3][0]); + data_write(data_out, 20, 1, imu_data[3][1]); + #endif + + #if DATA_MODE == 4 // ANALYSE IMU ACC VALUES + data_write(data_out, 0, 1, imu_data[0][3]); + data_write(data_out, 2, 1, imu_data[0][4]); + data_write(data_out, 4, 1, imu_data[0][5]); + data_write(data_out, 6, 1, imu_data[1][3]); + data_write(data_out, 8, 1, imu_data[1][4]); + data_write(data_out, 10, 1, imu_data[1][5]); + data_write(data_out, 12, 1, imu_data[2][3]); + data_write(data_out, 14, 1, imu_data[2][4]); + data_write(data_out, 16, 1, imu_data[2][5]); + data_write(data_out, 18, 1, imu_data[3][3]); + data_write(data_out, 20, 1, imu_data[3][4]); + #endif + + data_out[22] = ( comm_ud.status_SE <<0 | comm_ud.status_M1R <<1 | comm_ud.status_M2R <<2 |\ + comm_ud.status_M1E <<3 | comm_ud.status_M2E <<4 | ( comm_ud.status_IDX1D & comm_ud.status_IDX2D ) <<5|\ + ((comm_ud.status_errcode >> 0) & 1) <<6 | ((comm_ud.status_errcode >> 1) & 1) <<7); // Safe system info bools in char + data_out[23] = (flags.system <<0 | flags.angle <<1 | flags.rate <<2 |\ + flags.control <<3 | flags.battery <<4 | flags.wifi <<5|\ + flags.standup1_done <<6 | flags.standup2_done <<7); // Safe system info bools in char + + + data_out[24] = *(char *)&battery_voltage; + data_out[25] = *(((char *)&battery_voltage) + 1); + data_out[26] = *(char *)&counter; + data_out[27] = *(((char *)&counter) + 1); + + /* SEND DATA */ + if ( flags.usb_control ) + { + if( m_usb_rx_available() ) + { + read_usb( &data_in[0] ); + } + // COMPUTE CRC AND SEND DATA TO PC + *crc_out_ptr = m_crc32( (uint8_t *)data_out, PACKET_LENGTH-4 ); + data_out[PACKET_LENGTH-2] = 0; + data_out[PACKET_LENGTH-1] = 0; // If !crc_2PC_OK set 'crc2' to 1 + m_usb_tx_char('X'); + usb_serial_write( data_out, PACKET_LENGTH ); + } + else + { + if ( new_data ) + { + // READ INCOMING WIFI MESSAGE + new_data = FALSE; + counter_lastwifi = counter; + m_green( TOGGLE ); + + data_in0 = data_in[0]; + m_rf_read( (char *) data_in, PACKET_LENGTH_IN ); + + //if ( (counter % 100) == 0 ){ data_in[4] = 'J'; } // DEBUG - Test CRC + crc_in_OK = check_crc( data_in ); + //if( !crc_in_OK ){ memset( data_in, 0, PACKET_LENGTH_IN ); } + if( !crc_in_OK ){ data_in[0] = data_in0; } // Set + } + + // COMPUTE CRC AND SEND DATA TO PC + *crc_out_ptr = m_crc32( (uint8_t *)data_out, PACKET_LENGTH-4 ); + m_rf_send( TXADDRESS, (char *) data_out, PACKET_LENGTH ); + } + + // CHECK LOOP TIMES + sys_flag = 0; + + loop_time = micros(FALSE); + flags.loop_finished = TRUE; + check_loop_time( &sys_flag, loop_time ); + + } // end if(tick) + } // end while() +} // end main + + +/************************************************************************** + * + * Interrupt Service routines + * + **************************************************************************/ +ISR(INT2_vect) // a command telegram coming in +{ + new_data = TRUE; +} + +ISR(TIMER1_COMPA_vect) // fires approximately every 1/LOOP_FREQ [s] +{ + tick = TRUE; +} + +ISR(TIMER3_OVF_vect) { + cycle_overflow_counter++; +} + + +/************************************************************************** + * + * Functions + * + **************************************************************************/ + +//char checksum(int *ptr, uint16_t sz) { +// char chk = 0; +// while (sz-- != 0) +// chk -= *ptr++; +// return chk; +//} + +bool check_crc( uint8_t *data_in_ptr ) +{ // Check crc of incoming message + uint8_t crc_in[4] = {0}; + uint32_t *crc_in_ptr = (uint32_t *)&crc_in[0]; + + *crc_in_ptr = m_crc32( data_in_ptr, PACKET_LENGTH_IN-4 ); + + bool crc_OK; + crc_OK = (( crc_in[0] == data_in_ptr[PACKET_LENGTH_IN-4] ) + && ( crc_in[1] == data_in_ptr[PACKET_LENGTH_IN-3] ) + && ( crc_in[2] == data_in_ptr[PACKET_LENGTH_IN-2] ) + && ( crc_in[3] == data_in_ptr[PACKET_LENGTH_IN-1] )); + + return crc_OK; +} + +void read_usb(uint8_t *buffer) +{ // Reads in chars if the first char is an 'X' + if(m_usb_rx_char() == 'X') // new packet incoming! + { + int i; + for(i=0; i < PACKET_LENGTH_IN; i++) + { + while(!m_usb_rx_available()); // wait for byte + buffer[i] = m_usb_rx_char(); + } + } +} + +// Write data into wifi package using big-endian +void data_write(uint8_t *packet, int index, float scaling, float data) { + + int temp = 0; + + temp = (int)(scaling * data); + packet[index] = *(((char *)&temp) + 1); + packet[index+1] = *(char *)&temp; +} + +//! \brief setup timers for compare match interrupt and cycle clock timer +void setup_timer() { + cli(); // disable interrupts + + TCCR1A = 0; // Reset timer registers and timer itself + TCCR1B = 0; // ^ + TCNT1 = 0; // ^ + + // Compare match register, 16MHz / PRESCALER / DES. IMU FREQ. + uint16_t prescaler = select_timer1_prescaler(); // select correct prescaler for timer + OCR1A = F_CPU / prescaler / LOOP_FREQ; + + // PWM (Fast PWM, top = OCR1A, update OCR1 at TOP, flag set on TOP) + set(TCCR1B, WGM13); // ^ + set(TCCR1B, WGM12); // ^ + set(TCCR1A, WGM11); // ^ + set(TCCR1A, WGM10); // ^ + + set(TIMSK1, OCIE1A); // Enable interrupts on timers + + // clock used to calculate cycle time, for micros function + set(TCCR3B, CS30); // prescale 1 + set(TIMSK3, TOIE3); // interrupt on overflow. + + sei(); // enable interrupts +} //! \end of setup_timer function + +//! \brief Selects correct CPU prescaler for timer needed. also sets registers on 32u4 +//! \return 16 bit unsigned value of prescaler. has to be used in OCRxx for correct freq. +uint16_t select_timer1_prescaler() { + long int fcpu = F_CPU; + long int loop_freq = LOOP_FREQ; + if ((65534) > (fcpu / loop_freq)) { + set(TCCR1B, CS10); + return 1; + } else if (((65534 * 8) > (fcpu / loop_freq)) && ((65534) <= (fcpu / loop_freq))) { + set(TCCR1B, CS11); + return 8; + } else if (((65534 * 64) > (fcpu / loop_freq)) && ((65334 * 8) <= (fcpu / loop_freq))) { + set(TCCR1B, CS11); + set(TCCR1B, CS10); + return 64; + } else if (((65334 * 256) > (fcpu / loop_freq)) && ((65534 * 64) <= (fcpu / loop_freq))) { + set(TCCR1B, CS12); + return 256; + } else if (((65534 * 1024) > (fcpu / loop_freq)) && ((65534 * 256) <= (fcpu / loop_freq))) { + set(TCCR1B, CS12); + set(TCCR1B, CS11); + set(TCCR1B, CS10); + return 1024; + } else { + return 0; + } +} //! \end of select_timer1_prescaler function + +//! \brief function to check loop duration in micro seconds +//! \param[in] stream, bool, if true, this will print the cycle time, otherwise not +//! \return amount of micro seconds the last time the function is called has lapsed +uint16_t micros(bool stream) { + uint16_t val = 0; + val = (float)(((long)cycle_overflow_counter * 65535) + (((long)TCNT3H) << 8) + ((long)TCNT3L)) / + 16; // this must be done twice somehow + val = (float)(((long)cycle_overflow_counter * 65535) + (((long)TCNT3H) << 8) + ((long)TCNT3L)) / 16; + cycle_overflow_counter = 0; + TCNT3H = 0x00; + TCNT3L = 0x00; + /* if (stream) { + m_usb_tx_string("\t"); + m_usb_tx_uint(val); + } */ + return val; +} //! \end of micros function + +//! \brief function to enable, calibrate and wait for motor 1 to be ready. +void init_calibrate_udriver(Commstruct *comm_ud) { + volatile bool udriver_calibrated; + udriver_calibrated = FALSE; + m_spi_cs_setup(MD); // setup udriver pin for SPI + m_wait(10 * INIT_WAIT); + setMode(SYSTEM_ENABLE); + setMode(MOTOR_1_ENABLE); + setMode(MOTOR_2_ENABLE); + (*comm_ud).currentLimit1 = MAX_CURRENT_MOTOR; + (*comm_ud).currentLimit2 = MAX_CURRENT_MOTOR; + m_green(OFF); + + while (!udriver_calibrated) { + if (tick) { + send_spi_udriver(comm_ud); // send stuff + tick = FALSE; + + if ( (*comm_ud).status_M1R && (*comm_ud).status_M2R) { + udriver_calibrated = TRUE; + //m_green(ON); + } + } + } +} //! \end of init_calibrate_udriver function + +void init(){ + cli(); // disable interrupts + m_green( ON ); + + m_clockdivide( 0 ); // 16 MHz operation + + // SETUP ANALOG PIN FOR VOLTAGE MEASUREMENT + // ADC INIT, see: http://medesign.seas.upenn.edu/index.php/Guides/MaEvArM-adc + set( ADMUX , REFS0 ); // voltage reference to Vcc + set( ADCSRA , ADPS2 ); // set ADC prescaler to /64 + set( ADCSRA , ADPS1 ); // ^ + + if ( PCB_VERSION == 0 ) + { + // Set F0 or F1 to read voltage measurements + set( DIDR0 , ADC0D ); // disable digital on F0 to read current measurements + // set( ADMUX , MUX0 ); // Connect F1 + } + else if ( PCB_VERSION == 3 ) + { + m_disableJTAG(); // let me use F4-7 as GPIO + + // Set B4 to read voltage measurements + set( DIDR2 , ADC11D ); // disable digital on B4 to read current measurements + set( ADCSRB , MUX5 ); // Start new sample on channel set in MUXn + set( ADMUX , MUX1 ); // ^ + set( ADMUX , MUX0 ); // ^ + + set( ADCSRA , ADATE ); // free-running mode + set( ADCSRA , ADEN ); // enable ADC + set( ADCSRA , ADSC ); // start first conversion + + // ENABLE POWER TO UDRIVER + //#ifdef MP + set( DDRD , 5 ); // Init pin that controls power to udriver + set( PORTD , 5 ); // Set pin high to power udriver + //#endif + } + + + // COMMUNICATION INIT + m_bus_init(); // enable mBUS + m_usb_init(); // Init. USB streaming library + m_rf_open( CHANNEL, RXADDRESS, PACKET_LENGTH ); // configure mRF module + + setup_timer(); // setup timer for desired frequency to poll IMUs + m_spi_init(); // initialize SPI library + m_spi_speed( SPI_125KHZ ); // slower SPI speed for initialization + sei(); // enable interrupts +} \ No newline at end of file diff --git a/firmware/M2-on-wheelbot/src/matrix_calculations.c b/firmware/M2-on-wheelbot/src/matrix_calculations.c new file mode 100644 index 0000000..7d045d5 --- /dev/null +++ b/firmware/M2-on-wheelbot/src/matrix_calculations.c @@ -0,0 +1,45 @@ +#include "matrix_calculations.h" + +void multiply_matrix_vector(int rows_A, int columns_A, float Mat_A[rows_A][columns_A], + float Vec_B[columns_A], float Mat_C[rows_A]) { + + /* + for (int i = 0; i < rows_A; i++) { // looping through row i of Matrix A + Mat_C[i] = 0.0; + for (int j = 0; j < columns_A; j++) { // looping through column j of Matrix A + Mat_C[i] += Mat_A[i][j] * Vec_B[j]; + } + } + */ + + Mat_C[0] = Mat_A[0][0] * Vec_B[0] + Mat_A[0][1] * Vec_B[1] + Mat_A[0][2] * Vec_B[2]; + Mat_C[1] = Mat_A[1][0] * Vec_B[0] + Mat_A[1][1] * Vec_B[1] + Mat_A[1][2] * Vec_B[2]; + Mat_C[2] = Mat_A[2][0] * Vec_B[0] + Mat_A[2][1] * Vec_B[1] + Mat_A[2][2] * Vec_B[2]; +} + +void multiply_matrix_vector_long(int rows_A, int columns_A, int32_t Mat_A[rows_A][columns_A], + int Vec_B[columns_A], int32_t Mat_C[rows_A]) { + /* + for (int i = 0; i < rows_A; i++) { // looping through row i of Matrix A + Mat_C[i] = 0.0; + for (int j = 0; j < columns_A; j++) { // looping through column j of Matrix A + Mat_C[i] += (Mat_A[i][j] * Vec_B[j] ); + } + } + */ + Mat_C[0] = Mat_A[0][0] * Vec_B[0] + Mat_A[0][1] * Vec_B[1] + Mat_A[0][2] * Vec_B[2]; + Mat_C[1] = Mat_A[1][0] * Vec_B[0] + Mat_A[1][1] * Vec_B[1] + Mat_A[1][2] * Vec_B[2]; + Mat_C[2] = Mat_A[2][0] * Vec_B[0] + Mat_A[2][1] * Vec_B[1] + Mat_A[2][2] * Vec_B[2]; +} + +void rot_b_IMU(int32_t R_b_IMU[3][3], float rotx, float rotz) { + R_b_IMU[0][0] = (long)( 10000 * cos(rotz) ); + R_b_IMU[0][1] = (long)( -10000 * cos(rotx)*sin(rotz) ); + R_b_IMU[0][2] = (long)( 10000 * sin(rotx)*sin(rotz) ); + R_b_IMU[1][0] = (long)( 10000 * sin(rotz) ); + R_b_IMU[1][1] = (long)( 10000 * cos(rotx)*cos(rotz) ); + R_b_IMU[1][2] = (long)( -10000 * sin(rotx)*cos(rotz) ); + R_b_IMU[2][0] = (long)( 0 ); + R_b_IMU[2][1] = (long)( 10000 * sin(rotx) ); + R_b_IMU[2][2] = (long)( 10000 * cos(rotx) ); +} \ No newline at end of file diff --git a/firmware/M2-on-wheelbot/src/state_estimation.c b/firmware/M2-on-wheelbot/src/state_estimation.c new file mode 100644 index 0000000..9fbcbbd --- /dev/null +++ b/firmware/M2-on-wheelbot/src/state_estimation.c @@ -0,0 +1,134 @@ +#include "state_estimation.h" + +void estimate_pivot_acc(float *acc_pivot_B, float *q, float *dq, float ddq4, FlagsStruct *flags) { + /* STATES + * q[0]: roll, q1 + * q[1]: pitch, q2 + * q[2]: yaw, q3 + * dq[3] :rate wheel 2, dq4 + */ + + float acc_pivot_I[3] = {0.0, 0.0, 0.0}; + + if ( (*flags).use_pivotAcc_CP ) + { + acc_pivot_I[0] += Rw*ddq4; + //acc_pivot_I[1] += Rw*dq[3]*dq[2]; + } + + if ( (*flags).use_pivotAcc_WC_vel ) + { + acc_pivot_I[0] += 2*Rw*cos(q[0])*dq[0]*dq[2]; + acc_pivot_I[1] += ( Rw*sin(q[0])*(dq[0]*dq[0] + dq[2]*dq[2]) + Rw*dq[3]*dq[2] ); + acc_pivot_I[2] += -Rw*cos(q[0])*dq[0]*dq[0]; + } + + // Transform acc_pivot into body-fixed frame + + acc_pivot_B[0] = cos(q[1]) * acc_pivot_I[0] + sin(q[1]) * ( sin(q[0]) * acc_pivot_I[1] - cos(q[0]) * acc_pivot_I[2] ); + acc_pivot_B[1] = cos(q[0]) * acc_pivot_I[1] + sin(q[0]) * acc_pivot_I[2]; + acc_pivot_B[2] = sin(q[1]) * acc_pivot_I[0] - cos(q[1]) * ( sin(q[0]) * acc_pivot_I[1] - cos(q[0]) * acc_pivot_I[2] ); + + /* + if ( (*flags).compPivotAcc_WC_acc ) + { + acc_pivot[0] += Rw*sin(q[0])*ddq3; + acc_pivot[1] += -Rw*cos(q[0])*ddq1; + acc_pivot[2] += -Rw*sin(q[0])*ddq1; + } + */ +} + +void estimate_pivot_acc_linearized(float *acc_pivot_B, float *q, float *dq, float ddq4, FlagsStruct *flags) { + /* STATES + * q[0]: roll, q1 + * q[1]: pitch, q2 + * q[2]: yaw, q3 + * dq[3] :rate wheel 2, dq4 + */ + + acc_pivot_B[0] = 0.0; + acc_pivot_B[1] = 0.0; + acc_pivot_B[2] = 0.0; + + if ( (*flags).use_pivotAcc_CP ) + { + acc_pivot_B[0] += cos(q[1]) *Rw*ddq4; // Note that cos(q[1]) shouuld be 1 + acc_pivot_B[2] += sin(q[1]) *Rw*ddq4; + //acc_pivot_B[0] += Rw*ddq4; // Note that cos(q[1]) shouuld be 1 + } + + if ( (*flags).use_pivotAcc_WC_vel ) + { + acc_pivot_B[0] += 2*Rw*dq[0]*dq[2]; + acc_pivot_B[1] += Rw*dq[3]*dq[2]; + acc_pivot_B[2] += (-1*Rw*q[0]*dq[0]); + } +} + +void attitude_estimate_accel(float *q_acc, float *acc_pivot_B, int16_t *imu_data) { + float M[NUM_DIM][NUM_SENSORS]; // IMU measurements in matrix form + float g_B[NUM_DIM]; // Gravitation vector estimate in body frame + + float x1_star[NUM_SENSORS] = {0.0163, 0.0163, 0.4837, 0.4837}; // Estimator gain + //float x1_star[NUM_SENSORS] = {0.0, 0.0, 0.5, 0.5}; // Estimator gain + + /* + for (int i = 0; i < NUM_SENSORS; i++) { + for (int j = 0; j < NUM_DIM; j++) { + M[j][i] = (float)(*(imu_data + i * NUM_IMU_DATA + j)) * 0.000598755 - acc_pivot_B[j]; + } + } + */ + + + M[0][0] = (float)(*(imu_data + 0 * NUM_IMU_DATA + 0)) * 0.000598755 - acc_pivot_B[0]; + M[1][0] = (float)(*(imu_data + 0 * NUM_IMU_DATA + 1)) * 0.000598755 - acc_pivot_B[1]; + M[2][0] = (float)(*(imu_data + 0 * NUM_IMU_DATA + 2)) * 0.000598755 - acc_pivot_B[2]; + + M[0][1] = (float)(*(imu_data + 1 * NUM_IMU_DATA + 0)) * 0.000598755 - acc_pivot_B[0]; + M[1][1] = (float)(*(imu_data + 1 * NUM_IMU_DATA + 1)) * 0.000598755 - acc_pivot_B[1]; + M[2][1] = (float)(*(imu_data + 1 * NUM_IMU_DATA + 2)) * 0.000598755 - acc_pivot_B[2]; + + M[0][2] = (float)(*(imu_data + 2 * NUM_IMU_DATA + 0)) * 0.000598755 - acc_pivot_B[0]; + M[1][2] = (float)(*(imu_data + 2 * NUM_IMU_DATA + 1)) * 0.000598755 - acc_pivot_B[1]; + M[2][2] = (float)(*(imu_data + 2 * NUM_IMU_DATA + 2)) * 0.000598755 - acc_pivot_B[2]; + + M[0][3] = (float)(*(imu_data + 3 * NUM_IMU_DATA + 0)) * 0.000598755 - acc_pivot_B[0]; + M[1][3] = (float)(*(imu_data + 3 * NUM_IMU_DATA + 1)) * 0.000598755 - acc_pivot_B[1]; + M[2][3] = (float)(*(imu_data + 3 * NUM_IMU_DATA + 2)) * 0.000598755 - acc_pivot_B[2]; + + + multiply_matrix_vector(NUM_DIM, NUM_SENSORS, M, x1_star, g_B); + + q_acc[0] = atan(g_B[1]/sqrt(g_B[0]*g_B[0] + g_B[2]*g_B[2])); + q_acc[1] = atan(-g_B[0]/g_B[2]); + +} // end of theta_estimate_accel function + +void compute_estimator_bias(FlagsStruct *flags, uint16_t counter, uint16_t *counter_standup, float *roll_bias, + float *roll_bias_sum, float *pitch_bias, float *pitch_bias_sum, float attitude[2]) { + // If angle is bigger than 20 degree (0.35 rad) switch off + if ( (*flags).bias == FALSE ) + { + m_red( ON ); + (*flags).bias = TRUE; + *counter_standup = counter; + } + if ( ( counter - *counter_standup ) == 1500) + { + m_red( OFF ); + *roll_bias = *roll_bias_sum /(float)1000; + *pitch_bias = *pitch_bias_sum /(float)1000; + // Ensure that control bias is smaller 6 degrees, to prevent control mayhem + if ( (fabs(*roll_bias) > 0.09) || (fabs(*pitch_bias) > 0.09) ) + { + (*flags).control = FALSE; + } + } + if ( ( ( counter - *counter_standup ) >= 500) && ( (counter - *counter_standup) < 1500) ) + { + *roll_bias_sum += attitude[0]; + *pitch_bias_sum += attitude[1]; + } +} \ No newline at end of file diff --git a/firmware/M2-on-wheelbot/src/udriver.c b/firmware/M2-on-wheelbot/src/udriver.c new file mode 100644 index 0000000..2e9f41f --- /dev/null +++ b/firmware/M2-on-wheelbot/src/udriver.c @@ -0,0 +1,262 @@ +#include "udriver.h" +/* + * Script: udriver.c + * ----------------- + * variable initialization and functions for SPI communication with uDriver + * + * Info: + * + */ + +/************************************************************************** + * + * Variables + * + **************************************************************************/ + +uint8_t mosi[MD_PACKET_SIZE] = {0}; +uint8_t miso[MD_PACKET_SIZE] = {0}; + +uint16_t desiredMode = 0; +uint8_t crc_check[4] = {0}; +uint32_t *crc_check_ptr = (uint32_t *)&crc_check[0]; +bool crcOK; +uint16_t boardStatus; +uint16_t communicationInputIndex; +//uint16_t timestamp; + +// MOSI POINTERS +uint16_t *mode = (uint16_t *)&mosi[0]; +//int32_t *refPosition1 = (int32_t *)&mosi[2]; +//int32_t* refPosition2 = (int32_t*)&mosi[6]; +// int16_t *refVelocity1 = (int16_t *)&mosi[10]; // DEBUG: Commented out as not needed for current control +// int16_t* refVelocity2 = (int16_t*)&mosi[12]; // DEBUG: Commented out as not needed for current control +int16_t *refCurrent1 = (int16_t *)&mosi[14]; +int16_t* refCurrent2 = (int16_t*)&mosi[16]; +//uint16_t* kp1 = (uint16_t*)&mosi[18]; +//uint16_t* kp2 = (uint16_t*)&mosi[20]; +//uint16_t* kd1 = (uint16_t*)&mosi[22]; +//uint16_t* kd2 = (uint16_t*)&mosi[24]; +uint8_t *currentSaturation1 = (uint8_t *)&mosi[26]; +uint8_t *currentSaturation2 = (uint8_t*)&mosi[27]; +uint16_t *sendIdx = (uint16_t *)&mosi[28]; +uint32_t *sendCrc = (uint32_t *)&mosi[30]; + +// MISO POINTERS +uint16_t *status = (uint16_t *)&miso[0]; +uint16_t *ts = (uint16_t *)&miso[2]; +int32_t *mPositionMotor1 = (int32_t *)&miso[4]; +int32_t *mPositionMotor2 = (int32_t*)&miso[8]; +int16_t *mVelocityMotor1 = (int16_t *)&miso[12]; +int16_t *mVelocityMotor2 = (int16_t*)&miso[14]; +int16_t *mcurrentMotor1 = (int16_t *)&miso[16]; +int16_t *mcurrentMotor2 = (int16_t*)&miso[18]; +uint16_t *r_a = (uint16_t *)&miso[20]; +uint16_t *r_b = (uint16_t *)&miso[22]; +//uint16_t *mADCMotor1 = (uint16_t *)&miso[24]; +//uint16_t *mADCMotor2 = (uint16_t*)&miso[26]; +uint16_t *receiveIndex = (uint16_t *)&miso[28]; +uint32_t *receiveCrc = (uint32_t *)&miso[30]; + +/************************************************************************** + * + * + * Functions + * + **************************************************************************/ + +//! \brief Set mode of uDriver board, this is cumulative, except for system/motor disable. check +//! https://atlas.is.localnet/confluence/pages/viewpage.action?pageId=64260519 +//! \param[in] setting, setting you want to send to the uDriver board +void setMode(uint16_t setting) { + // set mode bits in human readable commands + switch (setting) { + case SYSTEM_ENABLE: + desiredMode |= (uint16_t)SYSTEM_ENABLE; + break; + case SYSTEM_DISABLE: + desiredMode &= (uint16_t)SYSTEM_DISABLE; + break; + case MOTOR_1_ENABLE: + desiredMode |= (uint16_t)MOTOR_1_ENABLE; + break; + case MOTOR_1_DISABLE: + desiredMode &= (uint16_t)MOTOR_1_DISABLE; + break; + case MOTOR_2_ENABLE: + desiredMode |= (uint16_t)MOTOR_2_ENABLE; + break; + case MOTOR_2_DISABLE: + desiredMode &= (uint16_t)MOTOR_2_DISABLE; + break; + case ROLLOVER_ENABLE: + desiredMode |= (uint16_t)ROLLOVER_ENABLE; + break; + case ROLLOVER_DISABLE: + desiredMode &= (uint16_t)ROLLOVER_DISABLE; + break; + case MOTOR_1_INDEX_COMPENSATION_ENABLE: + desiredMode |= (uint16_t)MOTOR_1_INDEX_COMPENSATION_ENABLE; + break; + case MOTOR_1_INDEX_COMPENSATION_DISABLE: + desiredMode &= (uint16_t)MOTOR_1_INDEX_COMPENSATION_DISABLE; + break; + case MOTOR_2_INDEX_COMPENSATION_ENABLE: + desiredMode |= (uint16_t)MOTOR_2_INDEX_COMPENSATION_ENABLE; + break; + case MOTOR_2_INDEX_COMPENSATION_DISABLE: + desiredMode &= (uint16_t)MOTOR_2_INDEX_COMPENSATION_DISABLE; + break; + default: // set communication timeout in ms + if ((setting > 0) & (setting < 255)) // delay in ms + desiredMode |= setting; + } +} //! \end of setMode function + +//! \brief exchange data/commands with uDriver board +//! \param[in] incoming_comm_struct, a struct of type "Commstruct". see udriver.h +//! \return bool if the transfer/CRC is successful +bool send_spi_udriver(Commstruct *comm_ud) { + // reset the outbound packet + for (int i = 0; i < MD_PACKET_SIZE; i++) { + mosi[i] = 0; + } + + // load mosi buffer + *mode = flipu16(desiredMode); + + // set current in desired format, with correct resolution etc. + *refCurrent1 = flip16((int16_t)(( (*comm_ud).currentTargetMotor1 ) * (1 << 10))); + *currentSaturation1 = (uint8_t)(( (*comm_ud).currentLimit1 ) * (1 << 3)); + *refCurrent2 = flip16((int16_t)(( (*comm_ud).currentTargetMotor2 ) * (1 << 10))); + *currentSaturation2 = (uint8_t)(( (*comm_ud).currentLimit2 ) * (1 << 3)); + + // send SPI stuff and check incoming CRC + *sendCrc = flipu32(m_crc32((uint8_t *)mosi, 30)); + m_spi_shift_MD(MD, (uint8_t *)mosi, (uint8_t *)miso, MD_PACKET_SIZE); + *crc_check_ptr = flipu32(m_crc32((uint8_t *)miso, 30)); + crcOK = ((crc_check[0] == miso[32]) && (crc_check[1] == miso[33]) && (crc_check[2] == miso[30]) && + (crc_check[3] == miso[31])); + + + if (crcOK) // write values to public variables + { + + (*comm_ud).positionMotor1 = (2.0 * PI * ((float)(flip32(*mPositionMotor1)) / (float)((int32_t)1 << 24))); + (*comm_ud).positionMotor2 = (2.0 * PI * ((float)(flip32(*mPositionMotor2)) / (float)((int32_t)1 << 24))); + (*comm_ud).velocityMotor1 = + ((float)(2.0 * PI * 1000.0 / 60.0 * ((double)(flip16(*mVelocityMotor1)) / (double)((int32_t)1 << 11)))); + (*comm_ud).velocityMotor2 = + ((float)(2.0 * PI * 1000.0 / 60.0 * ((double)(flip16(*mVelocityMotor2)) / (double)((int32_t)1 << 11)))); + (*comm_ud).currentMotor1 = (((float)(flip16(*mcurrentMotor1)) / (float)((int32_t)1 << 10))); + (*comm_ud).currentMotor2 = (((float)(flip16(*mcurrentMotor2)) / (float)((int32_t)1 << 10))); + + //// ADCMotor2 = (int16_t)(3.3 * ((float)(flip16(*mADCMotor2)) / (float)((int32_t)1 << 16))); + //// + //// communicationInputIndex = flipu16(*receiveIndex); + //// + boardStatus = flipu16(*status); + // timestamp = flipu16(*ts); + + (*comm_ud).status_SE = (boardStatus >> 15) & 1; + (*comm_ud).status_M1E = (boardStatus >> 14) & 1; + (*comm_ud).status_M1R = (boardStatus >> 13) & 1; + (*comm_ud).status_M2E = (boardStatus >> 12) & 1; + (*comm_ud).status_M2R = (boardStatus >> 11) & 1; + (*comm_ud).status_IDX1D = (boardStatus >> 10) & 1; + (*comm_ud).status_IDX2D = (boardStatus >> 9) & 1; + (*comm_ud).status_IDX1T = (boardStatus >> 8) & 1; + (*comm_ud).status_IDX2T = (boardStatus >> 7) & 1; + (*comm_ud).status_errcode = boardStatus & 0x07; + } + return crcOK; +} + +void check_motor_signs(Commstruct *pcomm, uint16_t *p_sys_flag, float *sign_motor1, float *sign_motor2, bool (*send_spi_udriver)(Commstruct *)) { + // Is a positive current creating a positive torque? + + float tmp1 = 0.0; + float tmp2 = 0.0; + uint16_t count; + + (*send_spi_udriver)( pcomm ); + tmp1 = (*pcomm).positionMotor1; + tmp2 = (*pcomm).positionMotor2; + + count = 0; + while(true) { + (*pcomm).currentTargetMotor1 += 0.01; + (*send_spi_udriver)( pcomm ); + fabs( (*pcomm).positionMotor1 - tmp1 ); + count += 1; + m_wait(10); + + if ( fabs( (*pcomm).positionMotor1 - tmp1 ) > 0.1 ) { + break; + } + if ( count > 300 ){ + *p_sys_flag = 1; + break; + } + } + (*pcomm).currentTargetMotor1 = 0.0; + (*send_spi_udriver)( pcomm ); + + count = 0; + while(true) { + (*pcomm).currentTargetMotor2 += 0.01; + (*send_spi_udriver)( pcomm ); + fabs( (*pcomm).positionMotor2 - tmp2 ); + count += 1; + m_wait(10); + + if ( fabs( (*pcomm).positionMotor2 - tmp2 ) > 0.1 ) { + break; + } + if ( count > 300 ){ + *p_sys_flag = 2; + break; + } + } + (*pcomm).currentTargetMotor2 = 0.0; + (*send_spi_udriver)( pcomm ); + + if ( (*pcomm).positionMotor1 > tmp1) { + *sign_motor1 = 1.0; + } + else if ((*pcomm).positionMotor1 < tmp1){ + *sign_motor1 = -1.0; + } + else { + *sign_motor1 = 1.0; + } + + if ( (*pcomm).positionMotor2 > tmp2) { + *sign_motor2 = 1.0; + } + else if ( (*pcomm).positionMotor2 < tmp2 ){ + *sign_motor2 = -1.0; + } + else { + *sign_motor2 = 1.0; + } + } + +// switch bytes to low byte first +uint16_t flipu16(uint16_t val) { return (val << 8) | (val >> 8); } + +// switch bytes to low byte first +int16_t flip16(int16_t val) { return (val << 8) | ((val >> 8) & 0xFF); } + +// switch bytes to low byte first +uint32_t flipu32(uint32_t val) { + val = ((val << 8) & 0xFF00FF00) | ((val >> 8) & 0xFF00FF); + return (val << 16) | (val >> 16); +} + +// switch bytes to low byte first +int32_t flip32(int32_t val) { + val = ((val << 8) & 0xFF00FF00) | ((val >> 8) & 0xFF00FF); + return (val << 16) | ((val >> 16) & 0xFFFF); +} + diff --git a/firmware/M2-wifi-dongle/Makefile b/firmware/M2-wifi-dongle/Makefile new file mode 100644 index 0000000..538cccb --- /dev/null +++ b/firmware/M2-wifi-dongle/Makefile @@ -0,0 +1,66 @@ +# -------------------------------------------------------- +# Custom M2 Makefile +# written by: Jonathan Bohren & Jonathan Fiene +# updated: July 6, 2011 +# -------------------------------------------------------- + +# -------------------------------------------------------- +# if you write separate C files to include in main +# add their .o targets to the OBJECTS line below +# (e.g. "OBJECTS = main.o myfile.o") +# -------------------------------------------------------- +OBJECTS = main.o m_usb.o m_bus.o m_rf.o m_crc32.o + +# -------------------------------------------------------- +# if you need to use one of our pre-compiled libraries, +# add it to the line below (e.g. "LIBRARIES = libsaast.a") +# -------------------------------------------------------- + +# -------------------------------------------------------- +# Default settings for the M2: +# -------------------------------------------------------- +DEVICE = atmega32u4 +CLOCK = 16000000 + +# -------------------------------------------------------- +# you shouldn't change anything below here, +# unless you really know what you're doing +# -------------------------------------------------------- + +COMPILE = avr-gcc -Wall -Os -DF_CPU=$(CLOCK) -mmcu=$(DEVICE) + +# symbolic targets: +all: main.hex + +.c.o: + $(COMPILE) -c $< -o $@ + +.S.o: + $(COMPILE) -x assembler-with-cpp -c $< -o $@ + +.c.s: + $(COMPILE) -S $< -o $@ + +install: flash + +flash: all + dfu-programmer atmega32u4 erase + dfu-programmer atmega32u4 flash main.hex + +clean: + rm -f main.hex main.elf $(OBJECTS) + +# file targets: +main.elf: $(OBJECTS) + $(COMPILE) -o main.elf $(OBJECTS) $(LIBRARIES) + +main.hex: main.elf + rm -f main.hex + avr-objcopy -j .text -j .data -O ihex main.elf main.hex + +# Targets for code debugging and analysis: +disasm: main.elf + avr-objdump -d main.elf + +cpp: + $(COMPILE) -E main.c diff --git a/firmware/M2-wifi-dongle/m_bus.c b/firmware/M2-wifi-dongle/m_bus.c new file mode 100644 index 0000000..e5591e6 --- /dev/null +++ b/firmware/M2-wifi-dongle/m_bus.c @@ -0,0 +1,116 @@ +// ----------------------------------------------------------------------------- +// M2 data bus subsystem +// version: 2.0 +// date: May 30, 2011 +// author: J. Fiene +// ----------------------------------------------------------------------------- + +#include "m_bus.h" + +// private function prototypes: +unsigned char twi_start(unsigned char address, unsigned char readwrite); +unsigned char twi_send_byte(unsigned char byte); +unsigned char twi_read_byte(void); +void twi_stop(void); +unsigned char twi_wait_for_ack(void); + + +// PUBLIC FUNCTIONS +void m_bus_init(void) +{ + // ENABLE PULLUPS + set(PORTD,0); + set(PORTD,1); + set(PORTD,2); + + // CONFIGURE THE CLOCK + TWBR = 12; // CLK freq = CPU clock / (16 + 2*TWBR*(4^TWPS)), 16MHz clock, TWBR=12, TWPS=00 -> 400kHz + + // ENABLE interrupts in INT2 (D2) + set(EICRA,ISC21); clear(EICRA,ISC20); // trigger on falling edge + set(EIMSK,INT2); // demask the interrupt + sei(); // enable global interrupts +} + +unsigned char m_read_register(unsigned char addr, unsigned char reg) +{ + if(!twi_start(addr,WRITE)) return 0; // START + W + if(!twi_send_byte(reg)) return 0; // register to read + twi_stop(); // STOP + if(!twi_start(addr,READ)) return 0; // START + R + return(twi_read_byte()); // return register value +} + +unsigned char m_write_register(unsigned char addr, unsigned char reg, unsigned char value) +{ + if(!twi_start(addr,WRITE)) return 0; // START + W + if(!twi_send_byte(reg)) return 0; // register to write to + if(!twi_send_byte(value)) return 0; // value + twi_stop(); // STOP + return(1); +} + +// PRIVATE FUNCTIONS: + +// TWI: send START condition, wait for ACK, send ADDRESS with R/W flag +// readwrite = 1 for read, 0 for write +unsigned char twi_start(unsigned char address, unsigned char readwrite) +{ + unsigned char status; + + // START packet: + TWCR = (1< + +#define MAX_WAIT 1000 +#define INTERPACKET 10 +#define READ 1 +#define WRITE 0 + +// ----------------------------------------------------------------------------- +// Public functions: +// ----------------------------------------------------------------------------- + +void m_bus_init(void); +// FUNCTIONALITY: +// initialize the M2 data bus, which uses pins D0-D2 and is available through the +// 5-pin end header. When new data is available from a slave, the INT2_vect interrupt +// will be triggered, and you must act accordingly! +// +// TAKES: +// nothing +// +// RETURNS: +// nothing + +unsigned char m_read_register(unsigned char addr, unsigned char reg); +// FUNCTIONALITY: +// sends [START + W] [register address] [STOP] [START + R] +// +// TAKES: +// addr - I2C slave address +// reg - register address +// +// RETURNS: +// register value + +unsigned char m_write_register(unsigned char addr, unsigned char reg, unsigned char value); +// FUNCTIONALITY: +// sends [START + W] [register address] [value] [STOP] +// +// TAKES: +// addr - I2C slave address +// reg - register address +// value - value to place in register +// +// RETURNS: +// 1 - success +// 0 - communication error + +#endif \ No newline at end of file diff --git a/firmware/M2-wifi-dongle/m_crc32.c b/firmware/M2-wifi-dongle/m_crc32.c new file mode 100644 index 0000000..56627ec --- /dev/null +++ b/firmware/M2-wifi-dongle/m_crc32.c @@ -0,0 +1,181 @@ +/* crc32.c + Copyright (C) 2009-2019 Free Software Foundation, Inc. + + This file is part of the libiberty library. + + This file is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation; either version 2 of the License, or + (at your option) any later version. + + In addition to the permissions in the GNU General Public License, the + Free Software Foundation gives you unlimited permission to link the + compiled version of this file into combinations with other programs, + and to distribute those combinations without any restriction coming + from the use of this file. (The General Public License restrictions + do apply in other respects; for example, they cover modification of + the file, and distribution when not linked into a combined + executable.) + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program; if not, write to the Free Software + Foundation, Inc., 51 Franklin Street - Fifth Floor, Boston, MA 02110-1301, USA. +*/ + + +/* This table was generated by the following program. + + #include + + int + main () + { + unsigned int i, j; + unsigned int c; + int table[256]; + + for (i = 0; i < 256; i++) + { + for (c = i << 24, j = 8; j > 0; --j) + c = c & 0x80000000 ? (c << 1) ^ 0x04c11db7 : (c << 1); + table[i] = c; + } + + printf ("static const unsigned int crc32_table[] =\n{\n"); + for (i = 0; i < 256; i += 4) + { + printf (" 0x%08x, 0x%08x, 0x%08x, 0x%08x", + table[i + 0], table[i + 1], table[i + 2], table[i + 3]); + if (i + 4 < 256) + putchar (','); + putchar ('\n'); + } + printf ("};\n"); + return 0; + } + + For more information on CRC, see, e.g., + http://www.ross.net/crc/download/crc_v3.txt. */ + +#include "m_crc32.h" + +static const uint32_t crc32_table[] = +{ + 0x00000000, 0x04c11db7, 0x09823b6e, 0x0d4326d9, + 0x130476dc, 0x17c56b6b, 0x1a864db2, 0x1e475005, + 0x2608edb8, 0x22c9f00f, 0x2f8ad6d6, 0x2b4bcb61, + 0x350c9b64, 0x31cd86d3, 0x3c8ea00a, 0x384fbdbd, + 0x4c11db70, 0x48d0c6c7, 0x4593e01e, 0x4152fda9, + 0x5f15adac, 0x5bd4b01b, 0x569796c2, 0x52568b75, + 0x6a1936c8, 0x6ed82b7f, 0x639b0da6, 0x675a1011, + 0x791d4014, 0x7ddc5da3, 0x709f7b7a, 0x745e66cd, + 0x9823b6e0, 0x9ce2ab57, 0x91a18d8e, 0x95609039, + 0x8b27c03c, 0x8fe6dd8b, 0x82a5fb52, 0x8664e6e5, + 0xbe2b5b58, 0xbaea46ef, 0xb7a96036, 0xb3687d81, + 0xad2f2d84, 0xa9ee3033, 0xa4ad16ea, 0xa06c0b5d, + 0xd4326d90, 0xd0f37027, 0xddb056fe, 0xd9714b49, + 0xc7361b4c, 0xc3f706fb, 0xceb42022, 0xca753d95, + 0xf23a8028, 0xf6fb9d9f, 0xfbb8bb46, 0xff79a6f1, + 0xe13ef6f4, 0xe5ffeb43, 0xe8bccd9a, 0xec7dd02d, + 0x34867077, 0x30476dc0, 0x3d044b19, 0x39c556ae, + 0x278206ab, 0x23431b1c, 0x2e003dc5, 0x2ac12072, + 0x128e9dcf, 0x164f8078, 0x1b0ca6a1, 0x1fcdbb16, + 0x018aeb13, 0x054bf6a4, 0x0808d07d, 0x0cc9cdca, + 0x7897ab07, 0x7c56b6b0, 0x71159069, 0x75d48dde, + 0x6b93dddb, 0x6f52c06c, 0x6211e6b5, 0x66d0fb02, + 0x5e9f46bf, 0x5a5e5b08, 0x571d7dd1, 0x53dc6066, + 0x4d9b3063, 0x495a2dd4, 0x44190b0d, 0x40d816ba, + 0xaca5c697, 0xa864db20, 0xa527fdf9, 0xa1e6e04e, + 0xbfa1b04b, 0xbb60adfc, 0xb6238b25, 0xb2e29692, + 0x8aad2b2f, 0x8e6c3698, 0x832f1041, 0x87ee0df6, + 0x99a95df3, 0x9d684044, 0x902b669d, 0x94ea7b2a, + 0xe0b41de7, 0xe4750050, 0xe9362689, 0xedf73b3e, + 0xf3b06b3b, 0xf771768c, 0xfa325055, 0xfef34de2, + 0xc6bcf05f, 0xc27dede8, 0xcf3ecb31, 0xcbffd686, + 0xd5b88683, 0xd1799b34, 0xdc3abded, 0xd8fba05a, + 0x690ce0ee, 0x6dcdfd59, 0x608edb80, 0x644fc637, + 0x7a089632, 0x7ec98b85, 0x738aad5c, 0x774bb0eb, + 0x4f040d56, 0x4bc510e1, 0x46863638, 0x42472b8f, + 0x5c007b8a, 0x58c1663d, 0x558240e4, 0x51435d53, + 0x251d3b9e, 0x21dc2629, 0x2c9f00f0, 0x285e1d47, + 0x36194d42, 0x32d850f5, 0x3f9b762c, 0x3b5a6b9b, + 0x0315d626, 0x07d4cb91, 0x0a97ed48, 0x0e56f0ff, + 0x1011a0fa, 0x14d0bd4d, 0x19939b94, 0x1d528623, + 0xf12f560e, 0xf5ee4bb9, 0xf8ad6d60, 0xfc6c70d7, + 0xe22b20d2, 0xe6ea3d65, 0xeba91bbc, 0xef68060b, + 0xd727bbb6, 0xd3e6a601, 0xdea580d8, 0xda649d6f, + 0xc423cd6a, 0xc0e2d0dd, 0xcda1f604, 0xc960ebb3, + 0xbd3e8d7e, 0xb9ff90c9, 0xb4bcb610, 0xb07daba7, + 0xae3afba2, 0xaafbe615, 0xa7b8c0cc, 0xa379dd7b, + 0x9b3660c6, 0x9ff77d71, 0x92b45ba8, 0x9675461f, + 0x8832161a, 0x8cf30bad, 0x81b02d74, 0x857130c3, + 0x5d8a9099, 0x594b8d2e, 0x5408abf7, 0x50c9b640, + 0x4e8ee645, 0x4a4ffbf2, 0x470cdd2b, 0x43cdc09c, + 0x7b827d21, 0x7f436096, 0x7200464f, 0x76c15bf8, + 0x68860bfd, 0x6c47164a, 0x61043093, 0x65c52d24, + 0x119b4be9, 0x155a565e, 0x18197087, 0x1cd86d30, + 0x029f3d35, 0x065e2082, 0x0b1d065b, 0x0fdc1bec, + 0x3793a651, 0x3352bbe6, 0x3e119d3f, 0x3ad08088, + 0x2497d08d, 0x2056cd3a, 0x2d15ebe3, 0x29d4f654, + 0xc5a92679, 0xc1683bce, 0xcc2b1d17, 0xc8ea00a0, + 0xd6ad50a5, 0xd26c4d12, 0xdf2f6bcb, 0xdbee767c, + 0xe3a1cbc1, 0xe760d676, 0xea23f0af, 0xeee2ed18, + 0xf0a5bd1d, 0xf464a0aa, 0xf9278673, 0xfde69bc4, + 0x89b8fd09, 0x8d79e0be, 0x803ac667, 0x84fbdbd0, + 0x9abc8bd5, 0x9e7d9662, 0x933eb0bb, 0x97ffad0c, + 0xafb010b1, 0xab710d06, 0xa6322bdf, 0xa2f33668, + 0xbcb4666d, 0xb8757bda, 0xb5365d03, 0xb1f740b4 +}; + +/* + +@deftypefn Extension {unsigned int} crc32 (const unsigned char *@var{buf}, @ + int @var{len}, unsigned int @var{init}) + +Compute the 32-bit CRC of @var{buf} which has length @var{len}. The +starting value is @var{init}; this may be used to compute the CRC of +data split across multiple buffers by passing the return value of each +call as the @var{init} parameter of the next. + +This is used by the @command{gdb} remote protocol for the @samp{qCRC} +command. In order to get the same results as gdb for a block of data, +you must pass the first CRC parameter as @code{0xffffffff}. + +This CRC can be specified as: + + Width : 32 + Poly : 0x04c11db7 + Init : parameter, typically 0xffffffff + RefIn : false + RefOut : false + XorOut : 0 + +This differs from the "standard" CRC-32 algorithm in that the values +are not reflected, and there is no final XOR value. These differences +make it easy to compose the values of multiple blocks. + +@end deftypefn + +*/ + +uint32_t m_crc32(uint8_t *buffer, int length) +{ + uint32_t crc = 0xFFFFFFFF; + while (length--) + { + crc = (crc << 8) ^ crc32_table[((crc >> 24) ^ *buffer) & 255]; + buffer++; + } + return crc; +} + +// switch bytes to low byte first +uint32_t flipu32(uint32_t val) { + val = ((val << 8) & 0xFF00FF00) | ((val >> 8) & 0xFF00FF); + return (val << 16) | (val >> 16); +} diff --git a/firmware/M2-wifi-dongle/m_crc32.h b/firmware/M2-wifi-dongle/m_crc32.h new file mode 100644 index 0000000..7f1c8aa --- /dev/null +++ b/firmware/M2-wifi-dongle/m_crc32.h @@ -0,0 +1,10 @@ +#pragma once + +#include "m_general.h" + + +// EXTERNAL FUNCTIONS + +uint32_t m_crc32 (uint8_t *buffer, int length); +uint32_t flipu32(uint32_t val); + diff --git a/firmware/M2-wifi-dongle/m_general.h b/firmware/M2-wifi-dongle/m_general.h new file mode 100644 index 0000000..658fe14 --- /dev/null +++ b/firmware/M2-wifi-dongle/m_general.h @@ -0,0 +1,114 @@ +// ----------------------------------------------------------------------------- +// mSYSTEM general functionality helper file +// version: 2.0 +// date: June 30, 2011 +// author: J. Fiene +// ----------------------------------------------------------------------------- +#ifndef m_general__ +#define m_general__ + +// ----------------------------------------------------------------------------- +// Useful pre-compile constants +// ----------------------------------------------------------------------------- + +#define TRUE 1 +#define FALSE 0 + +#define OFF 0 +#define ON 1 +#define TOGGLE 2 + +#define F0 0 +#define F1 1 +#define F4 4 +#define F5 5 +#define F6 6 +#define F7 7 +#define D4 8 +#define D6 9 +#define D7 10 +#define B4 11 +#define B5 12 +#define B6 13 + +#define B0 14 +#define B1 15 +#define B2 16 +#define B3 17 +#define B7 18 +#define D0 19 +#define D1 20 +#define D2 21 +#define D3 22 +#define D5 23 +#define C6 24 +#define C7 25 +#define E6 26 + + +// ----------------------------------------------------------------------------- +// General AVR libraries that we'll need at times: +// ----------------------------------------------------------------------------- + +#include +#include +#include +#include +#include +#include +#include + + +// ----------------------------------------------------------------------------- +// Bit manipulation and validation: +// ----------------------------------------------------------------------------- + +#define set(reg,bit) reg |= (1<<(bit)) +#define clear(reg,bit) reg &= ~(1<<(bit)) +#define toggle(reg,bit) reg ^= (1<<(bit)) + +#define check(reg,bit) (bool)(reg & (1<<(bit))) +// As of version 2.0, this will return either 1 (TRUE) or 0 (FALSE) + + +// ----------------------------------------------------------------------------- +// Disable JTAG to access F4-F7: +// ----------------------------------------------------------------------------- + +#define m_disableJTAG() MCUCR = (1 << JTD); MCUCR = (1 << JTD) +// Setting the JTD bit in MCUCR twice within four clock cycles will allow user +// access to F4-F7 as normal port pins. Note that using |= is too slow for this +// operation to work correctly, so we are setting the entire register +// (forutnately, all other bits in MCUCR are 0 anyway). + + +// ----------------------------------------------------------------------------- +// Set the system clock: +// ----------------------------------------------------------------------------- + +#define m_clockdivide(val) CLKPR = (1<> 8) & 255) + +#define HW_CONFIG() (UHWCON = 0x01) + +#ifdef M1 +#define PLL_CONFIG() (PLLCSR = 0x02) // fixed to 8MHz clock +#else +#define PLL_CONFIG() (PLLCSR = 0x12) // 0001 0010 For a 16MHz clock +#endif + +#define USB_CONFIG() (USBCON = ((1< size) write_size = size; + size -= write_size; + + // write the packet + switch (write_size) { + #if (CDC_TX_SIZE == 64) + case 64: UEDATX = *buffer++; + case 63: UEDATX = *buffer++; + case 62: UEDATX = *buffer++; + case 61: UEDATX = *buffer++; + case 60: UEDATX = *buffer++; + case 59: UEDATX = *buffer++; + case 58: UEDATX = *buffer++; + case 57: UEDATX = *buffer++; + case 56: UEDATX = *buffer++; + case 55: UEDATX = *buffer++; + case 54: UEDATX = *buffer++; + case 53: UEDATX = *buffer++; + case 52: UEDATX = *buffer++; + case 51: UEDATX = *buffer++; + case 50: UEDATX = *buffer++; + case 49: UEDATX = *buffer++; + case 48: UEDATX = *buffer++; + case 47: UEDATX = *buffer++; + case 46: UEDATX = *buffer++; + case 45: UEDATX = *buffer++; + case 44: UEDATX = *buffer++; + case 43: UEDATX = *buffer++; + case 42: UEDATX = *buffer++; + case 41: UEDATX = *buffer++; + case 40: UEDATX = *buffer++; + case 39: UEDATX = *buffer++; + case 38: UEDATX = *buffer++; + case 37: UEDATX = *buffer++; + case 36: UEDATX = *buffer++; + case 35: UEDATX = *buffer++; + case 34: UEDATX = *buffer++; + case 33: UEDATX = *buffer++; + #endif + #if (CDC_TX_SIZE >= 32) + case 32: UEDATX = *buffer++; + case 31: UEDATX = *buffer++; + case 30: UEDATX = *buffer++; + case 29: UEDATX = *buffer++; + case 28: UEDATX = *buffer++; + case 27: UEDATX = *buffer++; + case 26: UEDATX = *buffer++; + case 25: UEDATX = *buffer++; + case 24: UEDATX = *buffer++; + case 23: UEDATX = *buffer++; + case 22: UEDATX = *buffer++; + case 21: UEDATX = *buffer++; + case 20: UEDATX = *buffer++; + case 19: UEDATX = *buffer++; + case 18: UEDATX = *buffer++; + case 17: UEDATX = *buffer++; + #endif + #if (CDC_TX_SIZE >= 16) + case 16: UEDATX = *buffer++; + case 15: UEDATX = *buffer++; + case 14: UEDATX = *buffer++; + case 13: UEDATX = *buffer++; + case 12: UEDATX = *buffer++; + case 11: UEDATX = *buffer++; + case 10: UEDATX = *buffer++; + case 9: UEDATX = *buffer++; + #endif + case 8: UEDATX = *buffer++; + case 7: UEDATX = *buffer++; + case 6: UEDATX = *buffer++; + case 5: UEDATX = *buffer++; + case 4: UEDATX = *buffer++; + case 3: UEDATX = *buffer++; + case 2: UEDATX = *buffer++; + default: + case 1: UEDATX = *buffer++; + case 0: break; + } + // if this completed a packet, transmit it now! + if (!(UEINTX & (1<= NUM_DESC_LIST) { + UECONX = (1< desc_length) len = desc_length; + do { + // wait for host ready for IN packet + do { + i = UEINTX; + } while (!(i & ((1<= 1 && i <= MAX_ENDPOINT) { + usb_send_in(); + UENUM = i; + if (bRequest == SET_FEATURE) { + UECONX = (1<> 4); + phex1(c & 15); +} + +void m_usb_tx_hex(unsigned int i) +{ + phex(i >> 8); + phex(i); +} + +void m_usb_tx_hex8(unsigned char i) +{ + phex(i); +} + +void m_usb_tx_int(int i) +{ + char string[6] = {0,0,0,0,0,0}; + itoa(i,string,10); + for(i=0;i<6;i++){ + m_usb_tx_char(string[i]); + } +} + +void m_usb_tx_uint(unsigned int i) +{ + char string[5] = {0,0,0,0,0}; + utoa(i,string,10); + for(i=0;i<5;i++){ + m_usb_tx_char(string[i]); + } +} + +void m_usb_tx_long(long i) +{ + char string[11] = {0,0,0,0,0,0,0,0,0,0,0}; + ltoa(i,string,10); + for(i=0;i<11;i++){ + m_usb_tx_char(string[i]); + } +} + +void m_usb_tx_ulong(unsigned long i) +{ + char string[10] = {0,0,0,0,0,0,0,0,0,0}; + ultoa(i,string,10); + for(i=0;i<10;i++){ + m_usb_tx_char(string[i]); + } +} + diff --git a/firmware/M2-wifi-dongle/m_usb.h b/firmware/M2-wifi-dongle/m_usb.h new file mode 100644 index 0000000..2024172 --- /dev/null +++ b/firmware/M2-wifi-dongle/m_usb.h @@ -0,0 +1,186 @@ +// ----------------------------------------------------------------------------- +// M2 USB communication subsystem +// version: 2.0 +// date: June 30, 2011 +// authors: J. Fiene & J. Romano +// ----------------------------------------------------------------------------- + +#ifndef m_usb__ +#define m_usb__ + +#include "m_general.h" +#include + + +// ----------------------------------------------------------------------------- +// Public functions: +// ----------------------------------------------------------------------------- + +// INITIALIZATION: ------------------------------------------------------------- + +void m_usb_init(void); +// initialize the USB subsystem + +char m_usb_isconnected(void); +// confirm that the USB port is connected to a PC + + +// RECEIVE: ------------------------------------------------------------------- + +unsigned char m_usb_rx_available(void); +// returns the number of bytes (up to 255) waiting in the receive FIFO buffer + +char m_usb_rx_char(void); +// retrieve a oldest byte from the receive FIFO buffer (-1 if timeout/error) + +void m_usb_rx_flush(void); +// discard all data in the receive buffer + + + +// TRANSMIT: ------------------------------------------------------------------ + +char m_usb_tx_char(unsigned char c); +// add a single 8-bit unsigned char to the transmit buffer, return -1 if error + +void m_usb_tx_hex(unsigned int i); +// add an unsigned int to the transmit buffer, send as four hex-value characters + +void m_usb_tx_int(int i); +// add a signed int to the transmit buffer, send as a sign character then 5 decimal-value characters + +void m_usb_tx_uint(unsigned int i); +// add an unsigned int to the transmit buffer, send as 5 decimal-value characters + +void m_usb_tx_long(long i); +// add a signed long to the transmit buffer, send as a sign character then 5 decimal-value characters + +void m_usb_tx_ulong(unsigned long i); +// add an unsigned long to the transmit buffer, send as 5 decimal-value characters + +#define m_usb_tx_string(s) print_P(PSTR(s)) +// add a string to the transmit buffer + + + +// ----------------------------------------------------------------------------- +// ----------------------------------------------------------------------------- +// ----------------------------------------------------------------------------- + +// ---- OVERLOADS FOR M1 BACK COMPATIBILITY ---- +#define usb_init() m_usb_init() +#define usb_configured() m_usb_isconnected() + +#define usb_rx_available() m_usb_rx_available() +#define usb_rx_flush() m_usb_rx_flush() +#define usb_rx_char() m_usb_rx_char() + +#define usb_tx_char(val) m_usb_tx_char(val) +#define usb_tx_hex(val) m_usb_tx_hex(val) +#define usb_tx_decimal(val) m_usb_tx_uint(val) +#define usb_tx_string(val) m_usb_tx_string(val) +#define usb_tx_push() m_usb_tx_push() + +#define m_usb_rx_ascii() m_usb_rx_char() +#define m_usb_tx_ascii(val) m_usb_tx_char(val) + + +// EVERYTHING ELSE ***************************************************************** + +// setup + +int8_t usb_serial_putchar(uint8_t c); // transmit a character +int8_t usb_serial_putchar_nowait(uint8_t c); // transmit a character, do not wait +int8_t usb_serial_write(const uint8_t *buffer, uint16_t size); // transmit a buffer +void print_P(const char *s); +void phex(unsigned char c); +void phex16(unsigned int i); +void m_usb_tx_hex8(unsigned char i); +void m_usb_tx_push(void); + + +// serial parameters +uint32_t usb_serial_get_baud(void); // get the baud rate +uint8_t usb_serial_get_stopbits(void); // get the number of stop bits +uint8_t usb_serial_get_paritytype(void);// get the parity type +uint8_t usb_serial_get_numbits(void); // get the number of data bits +uint8_t usb_serial_get_control(void); // get the RTS and DTR signal state +int8_t usb_serial_set_control(uint8_t signals); // set DSR, DCD, RI, etc + +// constants corresponding to the various serial parameters +#define USB_SERIAL_DTR 0x01 +#define USB_SERIAL_RTS 0x02 +#define USB_SERIAL_1_STOP 0 +#define USB_SERIAL_1_5_STOP 1 +#define USB_SERIAL_2_STOP 2 +#define USB_SERIAL_PARITY_NONE 0 +#define USB_SERIAL_PARITY_ODD 1 +#define USB_SERIAL_PARITY_EVEN 2 +#define USB_SERIAL_PARITY_MARK 3 +#define USB_SERIAL_PARITY_SPACE 4 +#define USB_SERIAL_DCD 0x01 +#define USB_SERIAL_DSR 0x02 +#define USB_SERIAL_BREAK 0x04 +#define USB_SERIAL_RI 0x08 +#define USB_SERIAL_FRAME_ERR 0x10 +#define USB_SERIAL_PARITY_ERR 0x20 +#define USB_SERIAL_OVERRUN_ERR 0x40 + +// This file does not include the HID debug functions, so these empty +// macros replace them with nothing, so users can compile code that +// has calls to these functions. +#define usb_debug_putchar(c) +#define usb_debug_flush_output() + +#define EP_TYPE_CONTROL 0x00 +#define EP_TYPE_BULK_IN 0x81 +#define EP_TYPE_BULK_OUT 0x80 +#define EP_TYPE_INTERRUPT_IN 0xC1 +#define EP_TYPE_INTERRUPT_OUT 0xC0 +#define EP_TYPE_ISOCHRONOUS_IN 0x41 +#define EP_TYPE_ISOCHRONOUS_OUT 0x40 +#define EP_SINGLE_BUFFER 0x02 +#define EP_DOUBLE_BUFFER 0x06 +#define EP_SIZE(s) ((s) == 64 ? 0x30 : \ + ((s) == 32 ? 0x20 : \ + ((s) == 16 ? 0x10 : \ + 0x00))) + +#define MAX_ENDPOINT 4 + +#define LSB(n) (n & 255) +#define MSB(n) ((n >> 8) & 255) + +#define HW_CONFIG() (UHWCON = 0x01) + +#ifdef M1 + #define PLL_CONFIG() (PLLCSR = 0x02) // fixed to 8MHz clock +#else + #define PLL_CONFIG() (PLLCSR = 0x12) // 0001 0010 For a 16MHz clock +#endif + +#define USB_CONFIG() (USBCON = ((1< -clip ] + + fig, ax = plt.subplots() + + sns.lineplot(data=df_truncated[['Roll']], ci=None, ax=ax, palette=['red'], markers=True, markeredgecolor=None) # + sns.lineplot(data=df_truncated[['Pitch']], ci=None, ax=ax, palette=['blue'], markers=True, markeredgecolor=None) + + roll_mean = np.mean(df_truncated['Roll']) + pitch_mean = np.mean(df_truncated['Pitch']) + + plt.axhline(y=roll_mean, color='r', linestyle='-') + plt.axhline(y=pitch_mean, color='b', linestyle='-') + + print('roll_mean in rad : ', np.deg2rad(roll_mean)) + print('pitch_mean in rad: ', np.deg2rad(pitch_mean)) + #plt.xlabel('t [s]', fontsize=12) + #plt.ylabel('value', fontsize=12) + + # + #roll_mean in rad: -0.010795381105239055 + #pitch_mean in rad: 0.0036408923408705633 + + #roll_mean in rad: -0.012355374805888424 + #pitch_mean in rad: 0.01725571090312542 + + plt.show() + + + diff --git a/firmware/computer-python-interface/M2_pythonSerial.py b/firmware/computer-python-interface/M2_pythonSerial.py new file mode 100644 index 0000000..427f6a1 --- /dev/null +++ b/firmware/computer-python-interface/M2_pythonSerial.py @@ -0,0 +1,421 @@ +#!/usr/bin/env python +# Code based on: https://thepoorengineer.com/en/python-gui/ + +import sys +from threading import Thread +import serial +import time +from os import system, name +import collections +import matplotlib +import matplotlib.pyplot as plt +import matplotlib.animation as animation +import matplotlib.backends.backend_tkagg as backend_tkagg +import copy +import struct +import pandas as pd +import tkinter as Tk +from tkinter.ttk import Frame +matplotlib.use("TkAgg") +plt.style.use('bmh') +#plt.style.use('dark_background') + +class Window(Frame): + def __init__(self, figure, master, SerialReference): + Frame.__init__(self, master) + self.entry = None + self.setPoint = None + self.master = master # a reference to the master window + self.serialReference = SerialReference # keep a reference to our serial connection so that we can use it for bi-directional communicate from this class + self.initWindow(figure) # initialize the window with our settings + self.packet_length_2WB = 32 + self.data_out = ['0'] * self.packet_length_2WB # Last four bytes are reserved for CRC + self.data_out[9] = 'J' + + def initWindow(self, figure): + self.master.title("Real Time Plot") + canvas = backend_tkagg.FigureCanvasTkAgg(figure, master=self.master) + toolbar = backend_tkagg.NavigationToolbar2Tk(canvas, self.master) + canvas.get_tk_widget().pack(side=Tk.TOP, fill=Tk.BOTH, expand=1) + + # create out widgets in the master frame + lbl1 = Tk.Label(self.master, text="Enter command:") + lbl1.pack(padx=5, pady=5) + self.entry = Tk.Entry(self.master) + self.entry.insert(0, 'e') # (index, string) + self.entry.pack(padx=5) + + SendButton = Tk.Button(self.master, text='Send', command=self.sendCommand) + SendButton.pack(padx=5) + Tk.Button(self.master, text="Store data", command=self.master.quit).pack() + + StartButton = Tk.Button(self.master, text='Activate wheelbot', command=self.sendStartCommand) + StartButton.pack(side='right') + + StopButton = Tk.Button(self.master, text='STOP', command=self.sendStopCommand) + StopButton.pack(side='right') + + motor1clockwise = Tk.Button(self.master, text='motor1+', command=self.sendMotor1Command) + motor1clockwise.pack(side='left') + + #self.canvas.bind("", self.up) + + #def up(self, event): + # self.serialReference.sendSerialData("") + + def sendCommand(self): + self.data_out[0] = self.entry.get() + self.serialReference.sendSerialData(self.data_out) # '%' is our ending marker + + def sendStartCommand(self): + self.data_out[0] = 'i' + self.serialReference.sendSerialData(self.data_out) # '%' is our ending marker + + def sendStopCommand(self): + self.data_out[0] = 'e' + self.serialReference.sendSerialData(self.data_out) # '%' is our ending marker + + def sendMotor1Command(self): + self.data_out[0] = 'm' + self.serialReference.sendSerialData(self.data_out) # '%' is our ending marker + + +class serialPlot: + def __init__(self, unscale_list, plotLength=100, numPlots=8, select=range(16)): + self.serialPort0 = '/dev/ttyACM0' + self.serialBaud = 9600 + + self.unscale_list = unscale_list + self.plotMaxLength = plotLength + self.packetlength = 16 # A received package has length of 16 bytes + self.numPlots = numPlots + self.select = select + self.rawData = bytearray(self.packetlength * 2) + self.dataType = None + self.check = 0 + + self.dataType = ['!h'] * 16 # signed short (2 byte), the exclamation mark changes the byte order to big-endian + #self.dataType[12] = 'H' # The udriver flags are booleans stored in a 16 bit char (unsigned) + self.dataType[11] = 'H' # The safety flags are booleans stored in a 16 bit char, I convert it into unsigned short and then format it into a bit-string + self.dataType[12] = 'H' # Battery voltage (2 byte) is unsigned + self.dataType[13] = 'H' # Time measurements are unsigned + #self.value = [0] * self.packetlength + self.data = [0] * 16 + self.live_data = [] + for i in range(self.packetlength): # give an array for each type of data and store them in a list + self.live_data.append(collections.deque([0] * self.plotMaxLength, maxlen=self.plotMaxLength)) + self.isRun = True + self.thread = None + self.plotTimer = 0 + self.previousTimer = 0 + self.csvData = [] + + print('Trying to connect to: ' + str(self.serialPort0) + ' at ' + str(self.serialBaud) + ' BAUD.') + try: + self.serialConnection = serial.Serial(port=self.serialPort0, baudrate=self.serialBaud, bytesize=8, timeout=1, write_timeout=0) + print('Connected to ' + str(self.serialPort0) + ' at ' + str(self.serialBaud) + ' BAUD.') + except: + print("Failed to connect with " + str(self.serialPort0) + ' at ' + str(self.serialBaud) + ' BAUD.') + sys.exit() + + def readSerialStart(self): + if self.thread == None: + self.thread = Thread(target=self.backgroundThread) + self.thread.start() + #self.serialConnection.reset_input_buffer() + self.serialConnection.flush() + + # Block till we start receiving values + while ( self.serialConnection.inWaiting() < (32+1) ): + time.sleep(0.1) + + + def getSerialData(self, frame, lines, lineValueText, lineLabel, timeText): + currentTimer = time.perf_counter() + self.plotTimer = int((currentTimer - self.previousTimer) * 1000) # the first reading will be erroneous + self.previousTimer = currentTimer + timeText.set_text('Plot Interval = ' + str(self.plotTimer) + 'ms') + + for j in range(self.numPlots): + self.live_data[j].append(self.data[j]) + lines[j].set_data(range(self.plotMaxLength), self.live_data[self.select[j]]) + lineValueText[j].set_text('[' + lineLabel[j] + '] = ' + "{:.2f}".format(self.data[self.select[j]])) + + # ******************************* + # Print M2 STATUS + # ******************************* + U0, U1, U2, U3, U4, U5, U6, U7, M0, M1, M2, M3, M4, M5, M6, M7 = self.decode_booleans(self.data[11], 16) + + self.clear() # Function clears console print outs + print(f""" + Connected to: {str(self.serialPort0)} + M2 STATUS + # {'-' * 40} + # System : {"CHECK" if M0 else "X"} + # Angle : {"CHECK" if M1 else "X"} + # Rate : {"CHECK" if M2 else "X"} + # Control: {"CHECK" if M3 else "X"} + # Battery: {"CHECK" if M4 else "X"} + # Wifi : {"CHECK" if M5 else "X"} + # Standup: {"ON" if M6 else "OFF"} + # Balance: {"ON" if M7 else "OFF"} + # {'-' * 40} + + uDriver STATUS + # {'-' * 40} + # System enabled : {"CHECK" if U0 else "X"} + # Motor 1 ready : {"CHECK" if U1 else "X"} + # Motor 2 ready : {"CHECK" if U2 else "X"} + # Motor 1 enabled: {"CHECK" if U3 else "X"} + # Motor 2 enabled: {"CHECK" if U4 else "X"} + # Enc. detected : {"CHECK" if U5 else "X"} + # Error message : {int(U7), int(U6)} + # --> (0,1): Enc. error, (1,0): Comm. timeout + # {'-' * 40} + """, end='\r') + + # crc_2PC1 = self.float_to_bin(self.data[8]) + # crc_2PC2 = self.float_to_bin(self.data[9]) + # crc1 = self.float_to_bin(self.data[14]) + # crc2 = self.float_to_bin(self.data[15]) + # print('crc1 :', crc1) + # print('crc_2PC1:', crc_2PC1) + # print('crc2 :', crc2) + # print('crc_2PC2:', crc_2PC2) + # print('\n') + + + def float_to_bin(self, num): + return format(struct.unpack('!I', struct.pack('!f', num))[0], '032b') + + def sendSerialData(self, data): + data2 = ['X'] + data # The M2 checks if message received via USB starts with X + data2_string = ''.join([str(elem) for elem in data2]) + self.serialConnection.write(data2_string.encode()) + + def backgroundThread(self): # retrieve data + while (self.isRun): + #print(self.serialConnection.out_waiting) + + if self.serialConnection.inWaiting() > (63+2): + self.check = int.from_bytes(self.serialConnection.read(1), 'little') + + if self.check == 88 : # 88 is ASCII for 'X' + #print(check) + #print(self.serialConnection.inWaiting()) + + self.rawData = self.serialConnection.read(32) + #self.rawData = self.rawData[1:] + + for i in range(self.packetlength): + self.data[i] = self.unscale_list[i] * \ + struct.unpack(self.dataType[i], self.rawData[(i * 2):(i * 2 + 2)])[0] # struct.unpack: The result is a tuple even if it contains exactly one item. + + self.csvData.append([self.data[0], self.data[1], self.data[2], self.data[3], + self.data[4], self.data[5], self.data[6], self.data[7], + self.data[8], self.data[9], self.data[10], self.data[11], + self.data[12], self.data[13], self.data[14], self.data[15]]) + + def close(self, lineLabel): + self.isRun = False + self.thread.join() + self.serialConnection.close() + print('Disconnected...') + df = pd.DataFrame(self.csvData, columns=lineLabel) + df.to_csv('out.csv', encoding='utf-8', index=False, header='True') + + def encode_booleans(self, bool_lst): + # Code from Jan Vlcinsky, https://stackoverflow.com/questions/24038353/python-decoding-binary-to-boolean + res = 0 + for i, bval in enumerate(bool_lst): + res += int(bval) << i + return res + + def decode_booleans(self, intval, bits): + # Code from Jan Vlcinsky, https://stackoverflow.com/questions/24038353/python-decoding-binary-to-boolean + res = [] + for bit in range(bits): + mask = 1 << bit + res.append((intval & mask) == mask) + return res + + # define our clear function + def clear(self): + # Credit: https://www.geeksforgeeks.org/clear-screen-python/ + # for windows + if name == 'nt': + _ = system('cls') + + # for mac and linux(here, os.name is 'posix') + else: + _ = system('clear') + +def main(): + # portName = 'COM5' + maxPlotLength = 50 # number of points in x-axis of real time plot + + # Live plot for roll control + select = [0, 1, 2, 3] # Standard, Last byte has index 15 + #select = [8, 9, 10] # Last byte has index 15 + #select = [6, 9] # Last byte has index 15 + + # Live plot for pitch control + #select = [1, 3, 9, 10, 14] # Last byte has index 15 + #select = [4, 5, 6, 15] # Last byte has index 15 + #select = range(16) #[0, 1, 15] # Choose which data to + numPlots = len(select) # number of plots in 1 graph + + PI = 3.14159265358979 + unscaling_attitude = (180/PI)*(PI/32767) + unscaling_current = 40/32767 + unscaling_rate = 3000 / 32767 # bit value to rpm + gyro_scaling = 0.0002663161 # For gyro (500[deg/s] / 32768) * (PI / 180) so transforms bit value into rad/s + accel_scaling = 0.000598755 # 2*9,81 / 32768 # For accelerometer transforms bit in m/s^2 + + DATA_MODE = 1 + + unscale_list = [1] * 16 + #unscale_list[0] = unscale_angle + + if (DATA_MODE == 1): # Control plots + + flag_debug = 9 # 9 + + lineLabel = ['Roll', 'Pitch', 'Roll rate', 'Pitch rate', + 'I_sent1', 'I_sent2', 'rate_m1', 'rate_m2', + 'debug1', 'debug2', 'debug3', 'system_flags', + 'battery_voltage', 'time step', 'crc1', 'crc2'] + unscale_list[0:8] = [unscaling_attitude, # 0, roll + unscaling_attitude, # 1, pitch + gyro_scaling, # 2, roll rate + gyro_scaling , # 3, pitch rate + unscaling_current, # 5, current1 sent + unscaling_current, # 8, current2 sent + unscaling_rate, # 6, rate1 + unscaling_rate] # 9, rate2 + + if ( flag_debug == 1 ) : # MISC + unscale_list[8:11] = [unscaling_current, 200.0 / 32767, unscaling_rate] + elif ( flag_debug == 2 ) : # SYS FLAG + MOTOR POS + unscale_list[8:11] = [1.0, (10000*PI)/32767.0, (10000*PI)/32767.0] + elif ( flag_debug == 3 ) : # Flag, loop_time, tick_time + unscale_list[8:11] = [1.0, 2.0, 2.0] + elif ( flag_debug == 6 ) : # PIVOT ACC + unscale_list[8:11] = [200.0/32767, unscaling_rate, unscaling_current] + elif ( flag_debug == 7 ) : # ESTIMATOR BIAS + unscale_list[8:11] = [1.0, 1.8/32767, 1.8/32767] + elif (flag_debug == 8): # DATA IN + unscale_list[8:11] = [1.0, 1.0, 1.0] + elif (flag_debug == 9): # DATA IN + unscale_list[8:11] = [1.0, unscaling_current, unscaling_current] + elif (flag_debug == 10): # DATA IN + unscale_list[8:11] = [gyro_scaling, unscaling_attitude, unscaling_attitude] + elif (flag_debug == 11): # DATA IN + unscale_list[8:11] = [accel_scaling, accel_scaling, accel_scaling] + elif (flag_debug == 12): # DATA IN + unscale_list[8:11] = [unscaling_attitude, unscaling_attitude, unscaling_attitude] + elif (flag_debug == 13): # DATA IN + unscale_list[8:11] = [accel_scaling, accel_scaling, unscaling_rate] + + elif (DATA_MODE == 2): # Pivot acc plots + lineLabel = ['q1_acc', 'q2_acc', 'q1_acc_nopivot', 'q2_acc_nopivot', + 'rate_m2_filtered', 'rate_m2', + 'acc_imuC_x', 'acc_imuD_x', 'acc_imuC_y', + 'pivot_acc_x', 'pivot_acc_y', + 'system_flags', 'battery_voltage', 'time step', 'crc1', 'crc2'] + unscale_list[0:11] = [unscaling_attitude, + unscaling_attitude, + unscaling_attitude, + unscaling_attitude, + unscaling_rate, + unscaling_rate, + accel_scaling, + accel_scaling, + accel_scaling, + accel_scaling, + accel_scaling] + + elif (DATA_MODE == 3): # IMU accelerations + lineLabel = ['acc_imuA_x', 'acc_imuA_y', 'acc_imuA_z', + 'acc_imuB_x', 'acc_imuB_y', 'acc_imuB_z', + 'acc_imuC_x', 'acc_imuC_y', 'acc_imuC_z', + 'acc_imuD_x', 'acc_imuD_y', + 'system_flags', 'battery_voltage', 'time step', 'crc1', 'crc2'] + unscale_list[0:11] = [accel_scaling, + accel_scaling, + accel_scaling, + accel_scaling, + accel_scaling, + accel_scaling, + accel_scaling, + accel_scaling, + accel_scaling, + accel_scaling, + accel_scaling] + + elif (DATA_MODE == 4): # IMU rates + lineLabel = ['rate_imuA_x', 'rate_imuA_y', 'rate_imuA_z', + 'rate_imuB_x', 'rate_imuB_y', 'rate_imuB_z', + 'rate_imuC_x', 'rate_imuC_y', 'rate_imuC_z', + 'rate_imuD_x', 'rate_imuD_y', + 'system_flags', 'battery_voltage', 'time step', 'crc1', 'crc2'] + unscale_list[0:11] = [gyro_scaling, + gyro_scaling, + gyro_scaling, + gyro_scaling, + gyro_scaling, + gyro_scaling, + gyro_scaling, + gyro_scaling, + gyro_scaling, + gyro_scaling, + gyro_scaling] + + + + ## 11, are two bytes reserved for system checks + #unscale_list[12] = 1/30.09 # Battery voltage measurement + unscale_list[12] = 1 / 28.0 # Battery voltage measurement + + s = serialPlot(unscale_list, maxPlotLength, numPlots, select) # initializes all required variables + s.readSerialStart() # starts background thread + + pltInterval = 100 # Period at which the plot animation updates [ms] + xmin = 0 + xmax = maxPlotLength + ymin = -100 + ymax = 100 + fig = plt.figure(figsize=(10, 8)) + + # put our plot onto Tkinter's GUI + root = Tk.Tk() + app = Window(fig, root, s) + + ax = plt.axes(xlim=(xmin, xmax), ylim=(float(ymin - (ymax - ymin) / 10), float(ymax + (ymax - ymin) / 10))) + #ax.set_title('Wheelbot interface') + ax.set_xlabel("time steps") + ax.set_ylabel("data") + + lineLabel_plot = [lineLabel[i] for i in select] + + style = ['y'] * 16 + style[0:12] = ['r-', 'c-', 'b-', 'g-', 'y--', 'y:', 'y-', 'c:', 'r-', 'c-', 'b-', 'g-'] # linestyles for the different plots + style = [style[i] for i in select] + + timeText = ax.text(0.70, 0.95, '', transform=ax.transAxes) + lines = [] + lineValueText = [] + + for i in range(numPlots): + lines.append(ax.plot([], [], style[i], label=lineLabel_plot[i])[0]) + lineValueText.append(ax.text(0.70, 0.90-i*0.05, '', transform=ax.transAxes)) + + anim = animation.FuncAnimation(fig, s.getSerialData, fargs=(lines, lineValueText, lineLabel_plot, timeText), interval=pltInterval) # fargs has to be a tuple + + plt.legend(loc="upper left") + root.mainloop() # use this instead of plt.show() since we are encapsulating everything in Tkinter + s.close(lineLabel) + + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/firmware/computer-python-interface/Pipfile b/firmware/computer-python-interface/Pipfile new file mode 100644 index 0000000..bb20e8e --- /dev/null +++ b/firmware/computer-python-interface/Pipfile @@ -0,0 +1,17 @@ +[[source]] +name = "pypi" +url = "https://pypi.org/simple" +verify_ssl = true + +[dev-packages] + +[packages] +numpy = "*" +matplotlib = "*" +pandas = "*" +seaborn = "*" +pyserial = "*" +struct = "*" + +[requires] +python_version = "3.8"