136 lines
4.8 KiB
C++
136 lines
4.8 KiB
C++
#include <Arduino.h>
|
|
#include "globals.h"
|
|
#include "manipulator.h"
|
|
//#include <math.h>
|
|
|
|
// Utilities
|
|
|
|
manipulator_arm initializeManipulatorArm(manipulator_arm input) // Initialize an existing manipulator_arm struct
|
|
{
|
|
manipulator_arm out = input;
|
|
|
|
// Do initialization of values here
|
|
|
|
return out;
|
|
}
|
|
|
|
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
|
|
{
|
|
return (int)(servo_degrees * (90.0 / 127.0));
|
|
}
|
|
|
|
float clampServoAngle(float servo_angle) // Clamp an angle for a servo between -90.0 and 90.0
|
|
{
|
|
return max(-90.0f, min(90.0f, servo_angle));
|
|
}
|
|
|
|
|
|
// Tilt servo functions
|
|
|
|
manipulator_arm setTiltAngle(manipulator_arm input, float new_tilt_angle) // Internal function which sets the target angle of the tilt servo
|
|
{
|
|
manipulator_arm out = input;
|
|
new_tilt_angle = clampServoAngle(new_tilt_angle); // Clamp the new tilt angle
|
|
out.tilt_target_angle = new_tilt_angle;
|
|
out.tilt_set_motor_int = max(TILT_MIN_ANGLE, min(TILT_MAX_ANGLE, new_tilt_angle));
|
|
return out;
|
|
}
|
|
|
|
manipulator_arm updateTiltCommand(manipulator_arm input, int new_tilt_command) // Update the command for the tilt motor
|
|
{
|
|
manipulator_arm out = input;
|
|
if(new_tilt_command != out.tilt_command || out.tilt_angle_loops_since_update >= TILT_ANGLE_MIN_UPDATE_LOOPS) { // Make sure that it's been long enough since the last update to not spam the servo command
|
|
float new_angle = out.tilt_target_angle; // Maintain the existing angle by default
|
|
switch(new_tilt_command) {
|
|
case TILT_COMMAND_UNSET:
|
|
new_angle = out.tilt_target_angle; // Maintain the existing angle
|
|
case TILT_COMMAND_RESET:
|
|
new_angle = TILT_FLAT_ANGLE;
|
|
case TILT_COMMAND_UP:
|
|
new_angle = out.tilt_target_angle + TILT_ANGLE_UPDATE_DISTANCE;
|
|
break;
|
|
case TILT_COMMAND_DOWN:
|
|
new_angle = out.tilt_target_angle - TILT_ANGLE_UPDATE_DISTANCE;
|
|
break;
|
|
}
|
|
out = setTiltAngle(out, new_angle);
|
|
out.tilt_angle_loops_since_update = 0; // Reset the counter tracking loops since the last update, since an update was just performed
|
|
} else { // Increment the number of loops since the previous update, since an update was not performed during this loop
|
|
out.tilt_angle_loops_since_update++;
|
|
}
|
|
|
|
out.tilt_command = new_tilt_command; // Save the new command to check whether it has changed in the next loop
|
|
|
|
return out;
|
|
}
|
|
|
|
// 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 out = input;
|
|
out.claw_target_angle = clampServoAngle(new_claw_angle);
|
|
return out;
|
|
}
|
|
|
|
manipulator_arm updateClawCommand(manipulator_arm input, int new_claw_command) // Update the command and position for the claw servo
|
|
{
|
|
manipulator_arm out = input;
|
|
float new_claw_angle;
|
|
switch(new_claw_command) {
|
|
case CLAW_COMMAND_UNSET:
|
|
new_claw_angle = CLAW_DEFAULT_ANGLE;
|
|
break;
|
|
case CLAW_COMMAND_OPEN:
|
|
new_claw_angle = CLAW_OPEN_ANGLE;
|
|
break;
|
|
case CLAW_COMMAND_CLOSE:
|
|
new_claw_angle = CLAW_CLOSED_ANGLE;
|
|
break;
|
|
case CLAW_COMMAND_STAY:
|
|
new_claw_angle = out.claw_position;
|
|
break;
|
|
default:
|
|
new_claw_angle = CLAW_DEFAULT_ANGLE;
|
|
}
|
|
out.claw_position = new_claw_angle;
|
|
out.claw_set_motor_int = servoDegreesToInt(new_claw_angle);
|
|
return out;
|
|
}
|
|
|
|
|
|
// 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 out = input;
|
|
arm_speed = out.arm_speed_coefficient * min(out.arm_speed_limit, max(-out.arm_speed_limit, arm_speed));
|
|
out.arm_speed = arm_speed;
|
|
out.arm_set_motor_int = min(127, max(-127, (int) (127.0f * arm_speed)));
|
|
return out;
|
|
}
|
|
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 out = input;
|
|
out.arm_speed_limit = min(1.0f, max(0.0f, arm_speed_limit));
|
|
return out;
|
|
}
|
|
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
|
|
{
|
|
manipulator_arm out = input;
|
|
out.arm_speed_coefficient = min(1.0f, max(0.0f, arm_speed_coefficient));;
|
|
return out;
|
|
}
|
|
|
|
// Template function
|
|
|
|
manipulator_arm manipulatorTemplate(manipulator_arm input) // manipulator_arm template function
|
|
{
|
|
manipulator_arm out = input;
|
|
|
|
// Do stuff
|
|
|
|
return out;
|
|
}
|
|
|