39 lines
1.4 KiB
C
39 lines
1.4 KiB
C
#include "include/motion_control.h"
|
|
|
|
#include "include/motors.h"
|
|
#include "include/robot.h"
|
|
|
|
#define GAIN_KD 1
|
|
|
|
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 prcess them by paire of two
|
|
// - we calculate the error of the targeted angle and the actual angle
|
|
// - then we calculate motors speed from targeted speed, the error and the offset
|
|
|
|
float actual_angle = robot.gyro_data.z_angle; // Substracte 45* because robot is not oriented in the same angle as its moving base
|
|
float target_angle = (float)robot.motion_control_data.angle;
|
|
|
|
float error = target_angle - actual_angle;
|
|
float correction = error * GAIN_KD;
|
|
|
|
int8_t motor1_speed = robot.motion_control_data.x_axis_speed + correction;
|
|
int8_t motor2_speed = robot.motion_control_data.x_axis_speed - correction;
|
|
int8_t motor3_speed = robot.motion_control_data.y_axis_speed + correction;
|
|
int8_t motor4_speed = robot.motion_control_data.y_axis_speed - correction;
|
|
|
|
i2c_set_motor(MOTOR1, motor1_speed);
|
|
i2c_set_motor(MOTOR2, motor2_speed);
|
|
i2c_set_motor(MOTOR3, motor3_speed);
|
|
i2c_set_motor(MOTOR4, motor4_speed);
|
|
}
|