pimoroni-pico/examples/servo2040/servo2040_servo_wave.cpp

82 lines
1.9 KiB
C++

#include "pico/stdlib.h"
#include "servo2040.hpp"
#include "button.hpp"
/*
An example of applying a wave pattern to a group of servos and the LEDs.
Press "Boot" to exit the program.
*/
using namespace plasma;
using namespace servo;
// The speed that the LEDs will cycle at
const uint SPEED = 5;
// The brightness of the LEDs
constexpr float BRIGHTNESS = 0.4;
// How many times to update LEDs and Servos per second
const uint UPDATES = 50;
// How far from zero to move the servos
constexpr float SERVO_EXTENT = 80.0f;
// Create a servo cluster for pins 0 to 7, using PIO 0 and State Machine 0
const uint START_PIN = servo2040::SERVO_1;
const uint END_PIN = servo2040::SERVO_8;
const uint NUM_SERVOS = (END_PIN - START_PIN) + 1;
ServoCluster servos = ServoCluster(pio0, 0, START_PIN, NUM_SERVOS);
// Create the LED bar, using PIO 1 and State Machine 0
WS2812 led_bar(servo2040::NUM_LEDS, pio1, 0, servo2040::LED_DATA);
// Create the user button
Button user_sw(servo2040::USER_SW);
int main() {
stdio_init_all();
// Initialise the servo cluster
servos.init();
// Start updating the LED bar
led_bar.start();
float offset = 0.0f;
// Make rainbows until the user button is pressed
while(!user_sw.raw()) {
offset += (float)SPEED / 1000.0f;
// Update all the LEDs
for(auto i = 0u; i < servo2040::NUM_LEDS; i++) {
float hue = (float)i / (float)(servo2040::NUM_LEDS * 4);
led_bar.set_hsv(i, hue + offset, 1.0f, BRIGHTNESS);
}
// Update all the Servos
for(auto i = 0u; i < servos.count(); i++) {
float angle = (((float)i / (float)servos.count()) + offset) * (float)M_TWOPI;
servos.value(i, sin(angle) * SERVO_EXTENT, false);
}
// We have now set all the servo values, so load them
servos.load();
sleep_ms(1000 / UPDATES);
}
// Stop all the servos
servos.disable_all();
// Turn off the LED bar
led_bar.clear();
// Sleep a short time so the clear takes effect
sleep_ms(100);
}