kopia lustrzana https://github.com/pimoroni/pimoroni-pico
85 wiersze
2.1 KiB
C++
85 wiersze
2.1 KiB
C++
#include <cstdio>
|
|
#include "pico/stdlib.h"
|
|
|
|
#include "inventor.hpp"
|
|
|
|
/*
|
|
An example of how to move a servo smoothly between random positions.
|
|
|
|
Press "User" to exit the program.
|
|
*/
|
|
|
|
using namespace inventor;
|
|
|
|
// How many times to update Servos per second
|
|
const uint UPDATES = 50;
|
|
|
|
// The time to travel between each random value
|
|
const uint TIME_FOR_EACH_MOVE = 2;
|
|
const uint UPDATES_PER_MOVE = TIME_FOR_EACH_MOVE * UPDATES;
|
|
|
|
// How far from zero to move the servo
|
|
constexpr float SERVO_EXTENT = 80.0f;
|
|
|
|
// Whether or not to use a cosine path between values
|
|
const bool USE_COSINE = true;
|
|
|
|
|
|
// Create a new Inventor2040W
|
|
Inventor2040W board;
|
|
|
|
|
|
int main() {
|
|
stdio_init_all();
|
|
|
|
// Attempt to initialise the board
|
|
if(board.init()) {
|
|
|
|
// Access the servo from Inventor
|
|
Servo& s = board.servos[SERVO_1];
|
|
|
|
// Get the initial value and create a random end value between the extents
|
|
float start_value = s.mid_value();
|
|
float end_value = (((float)rand() / (float)RAND_MAX) * (SERVO_EXTENT * 2.0f)) - SERVO_EXTENT;
|
|
|
|
uint update = 0;
|
|
|
|
// Continually move the servo until the user button is pressed
|
|
while(!board.switch_pressed()) {
|
|
|
|
// Calculate how far along this movement to be
|
|
float percent_along = (float)update / (float)UPDATES_PER_MOVE;
|
|
|
|
if(USE_COSINE) {
|
|
// Move the servo between values using cosine
|
|
s.to_percent(cos(percent_along * (float)M_PI), 1.0, -1.0, start_value, end_value);
|
|
}
|
|
else {
|
|
// Move the servo linearly between values
|
|
s.to_percent(percent_along, 0.0, 1.0, start_value, end_value);
|
|
}
|
|
|
|
// Print out the value the servo is now at
|
|
printf("Value = %f\n", s.value());
|
|
|
|
// Move along in time
|
|
update++;
|
|
|
|
// Have we reached the end of this movement?
|
|
if(update >= UPDATES_PER_MOVE) {
|
|
// Reset the counter
|
|
update = 0;
|
|
|
|
// Set the start as the last end and create a new random end value
|
|
start_value = end_value;
|
|
end_value = (((float)rand() / (float)RAND_MAX) * (SERVO_EXTENT * 2.0f)) - SERVO_EXTENT;
|
|
}
|
|
|
|
sleep_ms(1000 / UPDATES);
|
|
}
|
|
|
|
// Disable the servo
|
|
s.disable();
|
|
}
|
|
}
|