Compare commits

..

2 Commits

2 changed files with 13 additions and 13 deletions

View File

@ -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;
}

View File

@ -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);
}