Added firmware used to run the robot

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

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

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

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 <math.h>
+
+/**************************************************************************
+ *
+ *  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