diff --git a/src/globals.h b/src/globals.h index dfb3052..bdfeb55 100644 --- a/src/globals.h +++ b/src/globals.h @@ -15,9 +15,12 @@ extern long last_p; */ // Loop timing -#define LOOP_DELAY_MS_CORE_0 50 // Minimum milliseconds between the start of each loop, accounting for processing time during each loop +// TODO TEMPORARY CHANGED TO 1000, NORMAL 50 +#define LOOP_DELAY_MS_CORE_0 5000 // Minimum milliseconds between the start of each loop, accounting for processing time during each loop +#define LOOP_DELAY_SECONDS_CORE_0 ((float)LOOP_DELAY_MS_CORE_0 / 1000.0f) // Previous number expressed in seconds + #define LOOP_DELAY_MS_CORE_1 50 // Minimum milliseconds between the start of each loop, accounting for processing time during each loop -#define LOOP_DELAY_SECONDS ((float)LOOP_DELAY_MS_CORE_0 / 1000.0f) // Previous number expressed in seconds +#define LOOP_DELAY_SECONDS_CORE_1 ((float)LOOP_DELAY_MS_CORE_1 / 1000.0f) // Previous number expressed in seconds // Math things #define DEGREES_PER_RADIAN (180.0 / M_PI) @@ -86,7 +89,7 @@ extern long last_p; // Tilt servo control parameters #define TILT_ANGLE_MIN_UPDATE_INTERVAL 0.2f // Update the tilt servo's target angle only after this many seconds have elapsed since the previous angle update -#define TILT_ANGLE_MIN_UPDATE_LOOPS (TILT_ANGLE_MIN_UPDATE_INTERVAL / LOOP_DELAY_SECONDS) // Previous value expressed as a number of control loops to wait between updates +#define TILT_ANGLE_MIN_UPDATE_LOOPS (TILT_ANGLE_MIN_UPDATE_INTERVAL / LOOP_DELAY_SECONDS_CORE_0) // Previous value expressed as a number of control loops to wait between updates #define TILT_ANGLE_UPDATE_DISTANCE 10.0f // Distance in degrees to shift the servo angle by each update #define TILT_MAX_ANGLE 90.0f // Maximum angle allowed for the tilt servo #define TILT_MIN_ANGLE -90.0f // Minimum angle allowed for the tilt servo diff --git a/src/main.cpp b/src/main.cpp index dd5dd2e..2c16ed0 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -973,6 +973,7 @@ void loop() { telemetry(zeroed_lx_float, zeroed_ly_float, zeroed_rx_float, zeroed_ry_float, loop_drive_mode, left_joystick_angle, left_joystick_magnitude, right_joystick_angle, right_joystick_magnitude); // DEBUG ONLY, print steering motor encoder positions // update motors after calculation + /* set_motor(FLSTEER, swrv.front_left_spin_power); set_motor(BRSTEER, swrv.back_right_spin_power); set_motor(FRSTEER, swrv.front_right_spin_power); @@ -981,6 +982,16 @@ void loop() { set_motor(BRDRIVE, swrv.back_right_power); set_motor(FRDRIVE, swrv.front_right_power); set_motor(BLDRIVE, swrv.back_left_power); + */ + for(int i = 0; i < 8; i++) { + if(i == loop_counter_core_0 % 8) { + set_motor(i + 1, 127); + Serial.print("Powering motor"); + Serial.println(i + 1); + } else { + set_motor(i + 1, 0); + } + } // update stepper motors set_motor(LIFTALL, clawarm.arm_set_motor_int); @@ -1145,7 +1156,7 @@ void loop() { previous_loop_processing_duration_core_0 = millis() - previous_loop_start_time_core_0; int64_t delay_time_ms = LOOP_DELAY_MS_CORE_0 - (int64_t) previous_loop_processing_duration_core_0; // Dynamically calculate delay time - if(delay_time_ms > 0 && delay_time_ms < 100) { // Only delay if the processing time has not exceeded LOOP_DELAY_MS_CORE_0 + if(delay_time_ms > 0/* && delay_time_ms < 100*/) { // Only delay if the processing time has not exceeded LOOP_DELAY_MS_CORE_0 delay(delay_time_ms); } loop_counter_core_0++;