pimoroni-pico/examples/inventor2040w/servos/inventor2040w_calibration.cpp

129 lines
4.3 KiB
C++

#include <cstdio>
#include "pico/stdlib.h"
#include "inventor.hpp"
/*
Shows how to configure Inventor 2040 W's servos with different
common calibrations, as well as a completely custom one.
*/
using namespace inventor;
// Create a new Inventor2040W
Inventor2040W board;
// Function for printing out the details of a Calibration object
void print_calibration(const Calibration &calib) {
printf("Calibration(");
uint size = calib.size();
printf("size = %d", size);
printf(", pairs = {");
for(uint i = 0; i < size; i++) {
const Calibration::Pair &pair = calib.pair(i);
printf("{%f, %f}",pair.pulse, pair.value);
if(i < size - 1)
printf(", ");
}
printf("}, limit_lower = %s", calib.has_lower_limit() ? "true" : "false");
printf(", limit_upper = %s", calib.has_upper_limit() ? "true" : "false");
printf(")\n\n");
}
int main() {
stdio_init_all();
// Attempt to initialise the board
if(board.init()) {
// Sleep 8 seconds to give enough time to connect up a terminal
sleep_ms(8000);
// Access four servos from Inventor
Servo& angular_servo = board.servos[SERVO_1];
Servo& linear_servo = board.servos[SERVO_2];
Servo& continuous_servo = board.servos[SERVO_3];
Servo& custom_servo = board.servos[SERVO_4];
// -----------------------------------------------------
// Create and modify the calibration of an angular servo
// -----------------------------------------------------
// Get a reference to the calibration and print it out
Calibration &acal = angular_servo.calibration();
printf("Angular Servo: ");
print_calibration(acal);
// The range we want the anglular servo to cover
constexpr float WIDE_ANGLE_RANGE = 270.0f;
// Lets modify the calibration to increase its range
acal.apply_default_pairs(ANGULAR);
acal.first_value(-WIDE_ANGLE_RANGE / 2);
acal.last_value(WIDE_ANGLE_RANGE / 2);
// As the calibration was a reference, the servo has already
// been updated, but lets access it again to confirm it worked
printf("Wide Angle Servo: ");
print_calibration(angular_servo.calibration());
// ---------------------------------------------------
// Create and modify the calibration of a linear servo
// ---------------------------------------------------
// The range we want the linear servo to cover
constexpr float LINEAR_RANGE = 50.0f;
// Update the linear servo so its max value matches the real distance travelled
Calibration &lcal = linear_servo.calibration();
lcal.apply_default_pairs(LINEAR);
lcal.last_value(LINEAR_RANGE);
// As the calibration was a reference, the servo has already
// been updated, but lets access it again to confirm it worked
printf("Linear Servo: ");
print_calibration(linear_servo.calibration());
// ----------------------------------------------------------------
// Create and modify the calibration of a continuous rotation servo
// ----------------------------------------------------------------
// The speed we want the continuous servo to cover
constexpr float CONTINUOUS_SPEED = 10.0f;
// Update the continuous rotation servo so its value matches its real speed
Calibration &ccal = continuous_servo.calibration();
ccal.apply_default_pairs(CONTINUOUS);
ccal.first_value(-CONTINUOUS_SPEED);
ccal.last_value(CONTINUOUS_SPEED);
// As the calibration was a reference, the servo has already
// been updated, but lets access it again to confirm it worked
printf("Continuous Servo: ");
print_calibration(continuous_servo.calibration());
// ------------------------------------------------------
// Create a custom calibration and build a servo using it
// ------------------------------------------------------
// Create an empty calibration
Calibration ecal = Calibration();
// Give it a range of -45 to 45 degrees, corresponding to pulses of 1000 and 2000 microseconds
ecal.apply_two_pairs(1000, 2000, -45, 45);
// Turn off the lower and upper limits, so the servo can go beyond 45 degrees
ecal.limit_to_calibration(false, false);
// Create a servo on pin 3 using the custom calibration and confirmed it worked
custom_servo.calibration() = ecal;
printf("Custom Servo: ");
print_calibration(custom_servo.calibration());
}
}