kopia lustrzana https://github.com/pimoroni/pimoroni-pico
96 wiersze
2.2 KiB
C++
96 wiersze
2.2 KiB
C++
#include "pico/stdlib.h"
|
|
|
|
#include "servo2040.hpp"
|
|
|
|
/*
|
|
Demonstrates how to create multiple Servo objects and control them together.
|
|
*/
|
|
|
|
using namespace servo;
|
|
|
|
// How many sweeps of the servo to perform
|
|
const uint SWEEPS = 3;
|
|
|
|
// The number of discrete sweep steps
|
|
const uint STEPS = 10;
|
|
|
|
// The time in milliseconds between each step of the sequence
|
|
const uint STEPS_INTERVAL_MS = 500;
|
|
|
|
// How far from zero to move the servo when sweeping
|
|
constexpr float SWEEP_EXTENT = 90.0f;
|
|
|
|
// Create an array of servo pointers
|
|
const uint START_PIN = servo2040::SERVO_1;
|
|
const uint END_PIN = servo2040::SERVO_4;
|
|
const uint NUM_SERVOS = (END_PIN - START_PIN) + 1;
|
|
Servo *servos[NUM_SERVOS];
|
|
|
|
|
|
int main() {
|
|
stdio_init_all();
|
|
|
|
// Fill the array of servos for pins 0 to 3, and initialise them. Up to 16 servos can be created
|
|
for(auto s = 0u; s < NUM_SERVOS; s++) {
|
|
servos[s] = new Servo(s + START_PIN);
|
|
servos[s]->init();
|
|
}
|
|
|
|
// Enable all servos (this puts them at the middle)
|
|
for(auto s = 0u; s < NUM_SERVOS; s++) {
|
|
servos[s]->enable();
|
|
}
|
|
sleep_ms(2000);
|
|
|
|
// Go to min
|
|
for(auto s = 0u; s < NUM_SERVOS; s++) {
|
|
servos[s]->to_min();
|
|
}
|
|
sleep_ms(2000);
|
|
|
|
// Go to max
|
|
for(auto s = 0u; s < NUM_SERVOS; s++) {
|
|
servos[s]->to_max();
|
|
}
|
|
sleep_ms(2000);
|
|
|
|
// Go back to mid
|
|
for(auto s = 0u; s < NUM_SERVOS; s++) {
|
|
servos[s]->to_mid();
|
|
}
|
|
sleep_ms(2000);
|
|
|
|
// Do a sine sweep
|
|
for(auto j = 0u; j < SWEEPS; j++) {
|
|
for(auto i = 0u; i < 360; i++) {
|
|
float value = sin(((float)i * (float)M_PI) / 180.0f) * SWEEP_EXTENT;
|
|
for(auto s = 0u; s < NUM_SERVOS; s++) {
|
|
servos[s]->value(value);
|
|
}
|
|
sleep_ms(20);
|
|
}
|
|
}
|
|
|
|
// Do a stepped sweep
|
|
for(auto j = 0u; j < SWEEPS; j++) {
|
|
for(auto i = 0u; i < STEPS; i++) {
|
|
for(auto s = 0u; s < NUM_SERVOS; s++) {
|
|
servos[s]->to_percent(i, 0, STEPS, 0.0 - SWEEP_EXTENT, SWEEP_EXTENT);
|
|
}
|
|
sleep_ms(STEPS_INTERVAL_MS);
|
|
}
|
|
for(auto i = 0u; i < STEPS; i++) {
|
|
for(auto s = 0u; s < NUM_SERVOS; s++) {
|
|
servos[s]->to_percent(i, STEPS, 0, 0.0 - SWEEP_EXTENT, SWEEP_EXTENT);
|
|
}
|
|
sleep_ms(STEPS_INTERVAL_MS);
|
|
}
|
|
}
|
|
|
|
// Disable the servos
|
|
for(auto s = 0u; s < NUM_SERVOS; s++) {
|
|
servos[s]->disable();
|
|
delete servos[s];
|
|
}
|
|
}
|