Compare commits
2 Commits
169f7c1103
...
b2995b9894
Author | SHA1 | Date |
---|---|---|
|
b2995b9894 | |
|
590ced4bd5 |
|
@ -79,9 +79,9 @@ void gyro_calibrate(void)
|
|||
robot.gyro_data.y_offset = (float)y_sum / (float)nb_samples * DPS_PER_DIGIT;
|
||||
robot.gyro_data.z_offset = (float)z_sum / (float)nb_samples * DPS_PER_DIGIT;
|
||||
|
||||
printf("\nx_cal:%.5f\n", robot.gyro_data.x_offset);
|
||||
printf("\ny_cal:%.5f\n", robot.gyro_data.y_offset);
|
||||
printf("\nz_cal:%.5f\n", robot.gyro_data.z_offset);
|
||||
//printf("\nx_cal:%.5f\n", robot.gyro_data.x_offset);
|
||||
//printf("\ny_cal:%.5f\n", robot.gyro_data.y_offset);
|
||||
//printf("\nz_cal:%.5f\n", robot.gyro_data.z_offset);
|
||||
}
|
||||
|
||||
static inline void __attribute__((always_inline)) gyro_get_dps(double* x_dps, double* y_dps, double* z_dps)
|
||||
|
@ -120,9 +120,9 @@ void gyro_update(void)
|
|||
while(robot.gyro_data.z_angle > 180) robot.gyro_data.z_angle -= 360;
|
||||
while(robot.gyro_data.z_angle < -180) robot.gyro_data.z_angle += 360;
|
||||
|
||||
printf(">gyro_x_angle:%f\n", robot.gyro_data.x_angle);
|
||||
printf(">gyro_y_angle:%f\n", robot.gyro_data.y_angle);
|
||||
printf(">gyro_z_angle:%f\n", robot.gyro_data.z_angle);
|
||||
//printf(">gyro_x_angle:%f\n", robot.gyro_data.x_angle);
|
||||
//printf(">gyro_y_angle:%f\n", robot.gyro_data.y_angle);
|
||||
//printf(">gyro_z_angle:%f\n", robot.gyro_data.z_angle);
|
||||
|
||||
elapsed_since_sample_ms = 0.0;
|
||||
}
|
||||
|
|
|
@ -25,11 +25,11 @@ void init_motors(void)
|
|||
const motor_def_t *motor_def = &MOTORS_DEFS[actual_motor];
|
||||
|
||||
// Init PWM
|
||||
uint slice_num = pwm_gpio_to_slice_num(motor_def->pwm_pin);
|
||||
const uint SLICE_NUM = pwm_gpio_to_slice_num(motor_def->pwm_pin);
|
||||
|
||||
gpio_set_function(motor_def->pwm_pin, GPIO_FUNC_PWM);
|
||||
pwm_set_wrap(slice_num, 128);
|
||||
pwm_set_enabled(slice_num, true);
|
||||
pwm_set_wrap(SLICE_NUM, 128);
|
||||
pwm_set_enabled(SLICE_NUM, true);
|
||||
|
||||
// Init dir pins
|
||||
gpio_init(motor_def->dir1_pin);
|
||||
|
@ -50,12 +50,12 @@ void init_servo_motors(void)
|
|||
const servo_motor_def_t *servo_motor_def = &SERVO_MOTORS_DEFS[actual_servo_motor];
|
||||
|
||||
// Init PWM //
|
||||
uint slice_num = pwm_gpio_to_slice_num(servo_motor_def->pwm_pin);
|
||||
const uint SLICE_NUM = pwm_gpio_to_slice_num(servo_motor_def->pwm_pin);
|
||||
|
||||
gpio_set_function(servo_motor_def->pwm_pin, GPIO_FUNC_PWM);
|
||||
pwm_set_wrap(slice_num, 25000);
|
||||
pwm_set_clkdiv(slice_num, 100);
|
||||
pwm_set_enabled(slice_num, true);
|
||||
pwm_set_wrap(SLICE_NUM, 25000);
|
||||
pwm_set_clkdiv(SLICE_NUM, 100);
|
||||
pwm_set_enabled(SLICE_NUM, true);
|
||||
|
||||
servo_motor_zero(actual_servo_motor);
|
||||
}
|
||||
|
|
Loading…
Reference in New Issue