void compute_pids() {
pid(PID_RATE_X)->input = imu_rates().x;
pid(PID_RATE_Y)->input = imu_rates().y;
pid(PID_ANGLE_X)->input = imu_angles().x;
pid(PID_ANGLE_Y)->input = imu_angles().y;
pid(PID_RATE_Z)->input = imu_rates().z;
if (flight_mode == STABILIZE) {
pid(PID_ANGLE_X)->setpoint = rc_get(RC_ROLL);
pid(PID_ANGLE_Y)->setpoint = rc_get(RC_PITCH);
pid_compute(PID_ANGLE_X);
pid_compute(PID_ANGLE_Y);
pid(PID_RATE_X)->setpoint = pid(PID_ANGLE_X)->output;
pid(PID_RATE_Y)->setpoint = pid(PID_ANGLE_Y)->output;
} else {
pid(PID_RATE_X)->setpoint = rc_get(RC_ROLL);
pid(PID_RATE_Y)->setpoint = rc_get(RC_PITCH);
}
pid(PID_RATE_Z)->setpoint = rc_get(RC_YAW);
pid_compute(PID_RATE_X);
pid_compute(PID_RATE_Y);
pid_compute(PID_RATE_Z);
}
void Motor::update()
{
cli();
// read and save current encoder ticks
float speed_left = encoder_left;
float speed_right = encoder_right;
encoder_left = encoder_right = 0;
// get elapsed time
uint32_t elapsed_time = timer_elapsed_time;
timer_elapsed_time = 0;
sei();
// convert into meters
speed_left *= MOTOR_METERS_PER_TICK;
speed_right *= MOTOR_METERS_PER_TICK;
// convert into seconds
float dt = (float)elapsed_time / 1000.0f;
// record odometry
left.odometry += speed_left;
left.odometry_t += dt;
right.odometry += speed_right;
right.odometry_t += dt;
if (dt <= 0.0f) return;
// trapezoidal speed control
if (left.target != left.goal)
{
// default to no acceleration limit
if (left.acceleration <= 0.0f)
{
left.target = left.goal;
}
else if (left.target < left.goal)
{
if (left.goal - left.target > left.acceleration * dt)
left.target += left.acceleration * dt;
else
left.target = left.goal;
}
else
{
if (left.target - left.goal > left.acceleration * dt)
left.target -= left.acceleration * dt;
else
left.target = left.goal;
}
}
if (right.target != right.goal)
{
// default to no acceleration limit
if (right.acceleration <= 0.0f)
{
right.target = right.goal;
}
else if (right.target < right.goal)
{
if (right.goal - right.target > right.acceleration * dt)
right.target += right.acceleration * dt;
else
right.target = right.goal;
}
else
{
if (right.target - right.goal > right.acceleration * dt)
right.target -= right.acceleration * dt;
else
right.target = right.goal;
}
}
if (left.powered)
{
if (left.pid.p == 0.0f)
{
// open loop
left.output = left.target;
}
else
{
// calculate speed
speed_left /= dt;
// pid
left.output += pid_compute(&left.pid, left.target - speed_left, dt);
}
}
else
{
// when power to the motor is lost, all is reset
left.pid.sum = 0.0f;
left.pid.e = 0.0f;
left.output = 0.0f;
left.target = 0.0f;
}
if (right.powered)
{
if (right.pid.p == 0.0f)
{
// open loop
right.output = right.target;
}
else
{
// calculate speed
speed_right /= dt;
// pid
right.output += pid_compute(&right.pid, right.target - speed_right, dt);
}
}
else
{
// when power to the motor is lost, all is reset
right.pid.sum = 0.0f;
right.pid.e = 0.0f;
right.output = 0.0f;
right.target = 0.0f;
}
// victor cap
if (left.output > MOTOR_SPEED_MAX)
left.output = MOTOR_SPEED_MAX;
else if (left.output < -MOTOR_SPEED_MAX)
left.output = -MOTOR_SPEED_MAX;
if (right.output > MOTOR_SPEED_MAX)
right.output = MOTOR_SPEED_MAX;
else if (right.output < -MOTOR_SPEED_MAX)
right.output = -MOTOR_SPEED_MAX;
// output to victor
victor_set_speed(left.output / MOTOR_SPEED_MAX * VICTOR_MAX, right.output / MOTOR_SPEED_MAX * VICTOR_MAX);
// read power
left.voltage = power_get_voltage_left();
left.current = power_get_current_left();
right.voltage = power_get_voltage_right();
right.current = power_get_current_right();
left.powered = left.voltage > MOTOR_POWER_THRESHOLD;
right.powered = right.voltage > MOTOR_POWER_THRESHOLD;
// motor ttl, stop motor when computer is not talking
if (left.ttl > dt)
left.ttl -= dt;
else
{
left.ttl = 0.0f;
left.goal = 0.0f;
}
if (right.ttl > dt)
right.ttl -= dt;
else
{
right.ttl = 0.0f;
right.goal = 0.0f;
}
}
void tick_task(const uint8_t tick_type) {
static int8_t message_counter = 0;
if(tick_type == TICK_TASK_TYPE_MESSAGE) {
if(usb_first_connection && !usbd_hal_is_disabled(IN_EP)) {
message_counter++;
if(message_counter >= 100) {
message_counter = 0;
if(brick_init_enumeration(COM_USB)) {
com_info.current = COM_USB;
usb_first_connection = false;
message_counter = 0;
}
}
}
}
if(tick_type == TICK_TASK_TYPE_CALCULATION) {
dc_tick_calc_counter++;
dc_current_sum += adc_channel_get_data(CURRENT_CONSUMPTION_CHANNEL);
if(dc_tick_calc_counter >= 100) {
dc_current = dc_current_sum/100;
dc_current_sum = 0;
dc_tick_calc_counter = 0;
}
dc_tick_counter++;
// Switch Output Voltage between extern and stack
if(dc_get_external_voltage() < DC_VOLTAGE_EPSILON) {
PIO_Set(&pin_voltage_switch);
} else {
PIO_Clear(&pin_voltage_switch);
}
if(!dc_enabled) {
return;
}
// Emit current velocity callback if necessary
if((dc_current_velocity_period != 0) &&
((dc_tick_counter % dc_current_velocity_period) == 0)) {
dc_current_velocity = true;
}
//if(!encoder_enabled) {
if(dc_velocity_goal == dc_velocity) {
return;
}
//}
if(dc_acceleration == 0) {
dc_velocity = dc_velocity_goal;
} else {
if(dc_velocity < dc_velocity_goal) {
dc_velocity = MIN(dc_velocity + dc_acceleration,
dc_velocity_goal);
} else {
dc_velocity = MAX(dc_velocity - dc_acceleration,
dc_velocity_goal);
}
}
#ifdef ENCODER
if(encoder_enabled) {
if(encoder_tick()) {
float setpoint = (dc_velocity/DC_VELOCITY_MULTIPLIER)*encoder_counts_per_revolution*pid.sample_time/(1000*60);
float out;
if(pid_compute(&pid, setpoint, (float)ABS(encoder_count_last), &out)) {
pid_velocity = (int32_t)out;
// pid_print(&pid);
}
}
}
#endif
// Set new velocity
dc_velocity_to_pwm();
// Emit velocity reachead callback
if(dc_velocity_goal == dc_velocity /*&& !encoder_enabled*/) {
dc_velocity_reached = true;
}
} else if(tick_type == TICK_TASK_TYPE_MESSAGE) {
dc_message_tick_counter++;
if(dc_velocity_reached) {
dc_velocity_reached = false;
dc_velocity_reached_callback();
}
if(dc_current_velocity) {
dc_current_velocity = false;
dc_current_velocity_callback();
}
dc_check_error_callbacks();
}
}