129 lines
4.3 KiB
C++
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());
|
|
}
|
|
}
|