65 lines
2.5 KiB
C
65 lines
2.5 KiB
C
#include "include/motion_control.h"
|
|
|
|
#include <stdio.h>
|
|
#include <math.h>
|
|
#include "include/motors.h"
|
|
#include "include/robot.h"
|
|
|
|
#define GAIN_KD 10
|
|
|
|
void init_motion_control(void)
|
|
{
|
|
robot.motion_control_data.angle = 0;
|
|
robot.motion_control_data.x_axis_speed = 0;
|
|
robot.motion_control_data.y_axis_speed = 0;
|
|
}
|
|
|
|
void i2c_update_motion_control(void)
|
|
{
|
|
// Motion control work as follow :
|
|
// - Motors are rotated on-board at 45*.
|
|
// - we calculate the error of the targeted angle and the actual angle
|
|
// - First we estimate the targeted speed on irl board on X and Y axis
|
|
// - Then we calculate motors speed from targeted speed and the error
|
|
// - And we put limits because motors speed are send by i2c on 1 byte
|
|
|
|
float actual_angle = robot.gyro_data.x_angle - 45.0f;
|
|
|
|
float target_angle = (float)robot.motion_control_data.angle - 45.0f;
|
|
float target_angle_radian = target_angle / 180.0f * M_PI;
|
|
|
|
float error = target_angle - actual_angle;
|
|
float correction = error * GAIN_KD;
|
|
|
|
float target_x_axis_speed = cosf(target_angle_radian) * robot.motion_control_data.x_axis_speed +
|
|
sinf(target_angle_radian) * robot.motion_control_data.y_axis_speed;
|
|
float target_y_axis_speed = cosf(target_angle_radian) * robot.motion_control_data.y_axis_speed -
|
|
sinf(target_angle_radian) * robot.motion_control_data.x_axis_speed;
|
|
|
|
int motor1_speed = target_x_axis_speed + (int)correction;
|
|
int motor2_speed = target_x_axis_speed - (int)correction;
|
|
int motor3_speed = target_y_axis_speed + (int)correction;
|
|
int motor4_speed = target_y_axis_speed - (int)correction;
|
|
|
|
if(motor1_speed > 127) motor1_speed = 127;
|
|
if(motor2_speed > 127) motor2_speed = 127;
|
|
if(motor3_speed > 127) motor3_speed = 127;
|
|
if(motor4_speed > 127) motor4_speed = 127;
|
|
|
|
if(motor1_speed < -128) motor1_speed = -128;
|
|
if(motor2_speed < -128) motor2_speed = -128;
|
|
if(motor3_speed < -128) motor3_speed = -128;
|
|
if(motor4_speed < -128) motor4_speed = -128;
|
|
|
|
i2c_set_motor(MOTOR1, motor1_speed);
|
|
i2c_set_motor(MOTOR2, motor2_speed);
|
|
i2c_set_motor(MOTOR3, motor3_speed);
|
|
i2c_set_motor(MOTOR4, motor4_speed);
|
|
}
|
|
|
|
void i2c_update_servo_motors(void)
|
|
{
|
|
for(servo_motors_enum_t actual_servo_motor = SERVO_MOTOR1; actual_servo_motor < NB_SERVO_MOTORS; actual_servo_motor++)
|
|
i2c_set_servo_motor(actual_servo_motor, robot.motion_control_data.servo_motors_pos[actual_servo_motor]);
|
|
}
|