Permalink
Switch branches/tags
Nothing to show
Find file Copy path
Fetching contributors…
Cannot retrieve contributors at this time
140 lines (111 sloc) 4.3 KB
// Author: Farrell Farahbod <farrellfarahbod@gmail.com>
// License: public domain
#include <stm32f0xx.h>
#include "f0lib/f0lib_mpu6050_hmc5883l.h"
#include "f0lib/f0lib_uart.h"
#include "f0lib/f0lib_timers.h"
#include "f0lib/f0lib_rf_cc2500.h"
#include "f0lib/f0lib_gpio.h"
#include "MadgwickAHRS.h"
#include <math.h>
#include <stdio.h>
// variables written to by the CC2500 packet received handler
volatile float gimbalX = 0;
volatile float gimbalY = 0;
volatile float knobLeft = 0;
volatile float knobMiddle = 0;
volatile float knobRight = 0;
void process_new_sensor_values(float gyro_x, float gyro_y, float gyro_z, float accel_x, float accel_y, float accel_z, float magn_x, float magn_y, float magn_z) {
// sensor fusion with Madgwick's Filter
// MadgwickAHRSupdate(gyro_z, gyro_y, -gyro_x, accel_z, accel_y, -accel_x, magn_z, magn_y, -magn_x);
MadgwickAHRSupdateIMU(gyro_z, gyro_y, -gyro_x, accel_z, accel_y, -accel_x);
// calculate the pitch angle so that: 0 = vertical -pi/2 = on its back +pi/2 = on its face
float pitch = asinf(-2.0f * (q1*q3 - q0*q2));
// calculate the set point (desired angle) and error (difference between the current angle and desired angle)
// since there are no wheel encoders, only throttle affects the set point
// mapping throttle to an angle so that: 0 = no throttle -pi/10 = full speed reverse +pi/10 = full speed forward
float set_point = (float) gimbalY / 1400.0f * 0.314159265f;
float error = pitch - set_point;
// calculate the proportional component (current error * p scalar)
float p_scalar = 12000.0f + (knobLeft - 2048.0f) * 5.90f;
if(p_scalar < 0) p_scalar = 0;
float proportional = error * p_scalar;
// calculate the integral component (summation of past errors * i scalar)
float i_scalar = 500.0f + (knobMiddle - 2048.0f) * 0.27f;
if(i_scalar < 0) i_scalar = 0;
static float integral = 0;
integral += error * i_scalar;
if(integral > 1000) integral = 1000; // limit wind-up
if(integral < -1000) integral = -1000;
// calculate the derivative component (change since previous error * d scalar)
static float previous_error = 0;
float d_scalar = 16000.0f + (knobRight - 2048.0f) * 7.85f;
if(d_scalar < 0) d_scalar = 0;
float derivative = (error - previous_error) * d_scalar;
previous_error = error;
int32_t motor_a_speed = proportional + integral + derivative;
int32_t motor_b_speed = proportional + integral + derivative;
// apply steering
motor_a_speed += gimbalX / 2;
motor_b_speed -= gimbalX / 2;
// stop the motors if we're far from vertical since there is no chance of success
if(pitch < -0.7f || pitch > 0.7f) {
motor_a_speed = 0;
motor_b_speed = 0;
}
timer_dual_hbridge_motor_speeds(motor_a_speed, motor_b_speed);
uart_send_bin_floats(27,
accel_x, // G
accel_y, // G
accel_z, // G
gyro_x, // Rad/s
gyro_y, // Rad/s
gyro_z, // Rad/s
magn_x, // Gs
magn_y, // Gs
magn_z, // Gs
pitch, // Rad
q0, // Quaternion
q1, // Quaternion
q2, // Quaternion
q3, // Quaternion
gimbalX,
gimbalY,
knobLeft,
knobMiddle,
knobRight,
set_point,
error,
p_scalar,
proportional,
i_scalar,
integral,
d_scalar,
derivative);
}
void process_new_packet(uint8_t byte_count, uint8_t bytes[]) {
int16_t gimX = (bytes[1] << 8) | bytes[0];
int16_t gimY = (bytes[3] << 8) | bytes[2];
int16_t knoL = (bytes[5] << 8) | bytes[4];
int16_t knoM = (bytes[7] << 8) | bytes[6];
int16_t knoR = (bytes[9] << 8) | bytes[8];
// ignore byte10: currently unused
gimbalX = (float) gimX;
gimbalY = (float) gimY;
knobLeft = (float) knoL;
knobMiddle = (float) knoM;
knobRight = (float) knoR;
}
void main(void) {
// configure the UART
uart_setup(PA9, 921600);
// configure the 9DOF
mpu6050_hmc5883l_setup(PB8, PB9, PB7, &process_new_sensor_values);
// configure the dual h-bridge PWM timer
timer_dual_hbridge_setup(PA0, PA1, PA2, PA3);
// configure the RF module
cc2500_setup(SPI1, PB3, PB4, PB5, PD2, PC12, 11, &process_new_packet);
cc2500_enter_rx_mode();
// everything else is handled by the ISR
while(1);
}