61 lines
3.3 KiB
C
61 lines
3.3 KiB
C
#include "globals.h"
|
|
|
|
// Manipulator arm
|
|
|
|
// Stepper motors for lift
|
|
// Servo to open and close claw
|
|
// Servo to tilt claw
|
|
|
|
typedef struct {
|
|
// Claw servo variables
|
|
int claw_command = CLAW_COMMAND_UNSET; // COMMAND for claw servo
|
|
int claw_set_motor_int = 0; // COMMAND for claw servo, formatted to integer used in set_motor()
|
|
int claw_status = CLAW_UNKNOWN; // CURRENT claw position, initially unknown
|
|
float claw_position = CLAW_DEFAULT_ANGLE; // CURRENT angle of claw servo
|
|
float claw_target_angle = CLAW_DEFAULT_ANGLE; // TARGET position of claw servo, -90.0 to 90.0 degrees
|
|
float claw_power_limit = 0.0f; // LIMIT power for the claw servo
|
|
|
|
// Tilt servo variables
|
|
int tilt_command = TILT_COMMAND_UNSET; // COMMAND for the tilt servo
|
|
float tilt_target_angle = TILT_FLAT_ANGLE; // CURRENT angle in degrees for the claw tilt servo, can be between -90.0 and 90.0 degrees
|
|
int tilt_set_motor_int = 0; // Tilt motor target angle value as an integer used for set_motor()
|
|
int tilt_angle_loops_since_update = 0; // Number of control loops since the tilt angle was modified, don't modify the tilt angle if this is less than TILT_ANGLE_MIN_UPDATE_LOOPS
|
|
|
|
// Arm motor variables (stepper motors)
|
|
float arm_speed = 0.0f; // Arm command, initialize as stopped, between -1.0 and 1.0
|
|
float arm_speed_limit = 1.0f; // Limit applied to the speed of the arm (before the coefficient), must be between 0.0 and 1.0
|
|
float arm_speed_coefficient = 1.0f; // Coefficient applied to the arm speed, has no constraints, default to 1.0 (unmodified)
|
|
int arm_set_motor_int = 0; // Arm speed used for the set_motor() function
|
|
|
|
} manipulator_arm;
|
|
|
|
|
|
// Utilities
|
|
|
|
manipulator_arm initializeManipulatorArm(manipulator_arm input); // Initialize a manipulator_arm struct
|
|
|
|
int servoDegreesToInt(float servo_degrees); // Convert a servo position in degrees to an integer between -127 and 127 to be used by the set_motor() function
|
|
|
|
float clampServoAngle(float servo_angle); // Clamp an angle for a servo between -90.0 and 90.0
|
|
|
|
// Tilt servo functions
|
|
manipulator_arm setTiltAngle(manipulator_arm input, float new_claw_angle); // Internal function which sets the target angle of the tilt servo
|
|
|
|
manipulator_arm updateTiltCommand(manipulator_arm input, int tilt_command); // Update the command for the tilt motor
|
|
|
|
// Claw servo functions
|
|
|
|
manipulator_arm setClawTargetAngle(manipulator_arm input, float new_claw_angle); // Internal function which sets the target angle of the claw servo (NOT THE ACTUAL COMMAND ANGLE)
|
|
|
|
manipulator_arm updateClawCommand(manipulator_arm input, int new_claw_command); // Update the command and position for the claw servo
|
|
|
|
// Arm functions (stepper motors)
|
|
|
|
manipulator_arm setArmSpeed(manipulator_arm input, float arm_speed); // Set the arm's speed, must be between -1.0 and 1.0
|
|
|
|
manipulator_arm setArmSpeedLimit(manipulator_arm input, float arm_speed_limit); // Set the arm's speed limit (applied before coefficient), limit must be between 0.0 and 1.0
|
|
|
|
manipulator_arm setArmSpeedCoefficient(manipulator_arm input, float arm_speed_coefficient); // Set the arm's speed coefficient, coefficient must be between 0.0 and 1.0
|
|
|
|
|