kopia lustrzana https://github.com/pimoroni/pimoroni-pico
106 wiersze
2.5 KiB
C++
106 wiersze
2.5 KiB
C++
#include <math.h>
|
|
#include "pico/stdlib.h"
|
|
|
|
#include "pico_motor_shim.hpp"
|
|
|
|
/*
|
|
Demonstrates how to create two Motor objects and control them together.
|
|
*/
|
|
|
|
using namespace motor;
|
|
|
|
// How many sweeps of the motors to perform
|
|
const uint SWEEPS = 2;
|
|
|
|
// 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 drive the motors when sweeping
|
|
constexpr float SPEED_EXTENT = 1.0f;
|
|
|
|
// Create an array of motor pointers
|
|
const pin_pair motor_pins[] = {pico_motor_shim::MOTOR_1, pico_motor_shim::MOTOR_2};
|
|
const uint NUM_MOTORS = count_of(motor_pins);
|
|
Motor *motors[NUM_MOTORS];
|
|
|
|
|
|
int main() {
|
|
stdio_init_all();
|
|
|
|
// Fill the array of motors, and initialise them. Up to 8 motors can be created
|
|
for(auto m = 0u; m < NUM_MOTORS; m++) {
|
|
motors[m] = new Motor(motor_pins[m]);
|
|
motors[m]->init();
|
|
}
|
|
|
|
// Uncomment the below lines to reverse
|
|
// the driving direction of a motor
|
|
// motors[0].direction(REVERSED_DIR);
|
|
// motors[1].direction(REVERSED_DIR);
|
|
|
|
// Enable all motors
|
|
for(auto m = 0u; m < NUM_MOTORS; m++) {
|
|
motors[m]->enable();
|
|
}
|
|
sleep_ms(2000);
|
|
|
|
// Drive at full positive
|
|
for(auto m = 0u; m < NUM_MOTORS; m++) {
|
|
motors[m]->full_positive();
|
|
}
|
|
sleep_ms(2000);
|
|
|
|
// Stop all moving
|
|
for(auto m = 0u; m < NUM_MOTORS; m++) {
|
|
motors[m]->stop();
|
|
}
|
|
sleep_ms(2000);
|
|
|
|
// Drive at full negative
|
|
for(auto m = 0u; m < NUM_MOTORS; m++) {
|
|
motors[m]->full_negative();
|
|
}
|
|
sleep_ms(2000);
|
|
|
|
// Coast all to a gradual stop
|
|
for(auto m = 0u; m < NUM_MOTORS; m++) {
|
|
motors[m]->coast();
|
|
}
|
|
sleep_ms(2000);
|
|
|
|
// Do a sine speed sweep
|
|
for(auto j = 0u; j < SWEEPS; j++) {
|
|
for(auto i = 0u; i < 360; i++) {
|
|
float speed = sin(((float)i * (float)M_PI) / 180.0f) * SPEED_EXTENT;
|
|
for(auto m = 0u; m < NUM_MOTORS; m++) {
|
|
motors[m]->speed(speed);
|
|
}
|
|
sleep_ms(20);
|
|
}
|
|
}
|
|
|
|
// Do a stepped speed sweep
|
|
for(auto j = 0u; j < SWEEPS; j++) {
|
|
for(auto i = 0u; i < STEPS; i++) {
|
|
for(auto m = 0u; m < NUM_MOTORS; m++) {
|
|
motors[m]->to_percent(i, 0, STEPS, 0.0 - SPEED_EXTENT, SPEED_EXTENT);
|
|
}
|
|
sleep_ms(STEPS_INTERVAL_MS);
|
|
}
|
|
for(auto i = 0u; i < STEPS; i++) {
|
|
for(auto m = 0u; m < NUM_MOTORS; m++) {
|
|
motors[m]->to_percent(i, STEPS, 0, 0.0 - SPEED_EXTENT, SPEED_EXTENT);
|
|
}
|
|
sleep_ms(STEPS_INTERVAL_MS);
|
|
}
|
|
}
|
|
|
|
// Disable the motors
|
|
for(auto m = 0u; m < NUM_MOTORS; m++) {
|
|
motors[m]->disable();
|
|
}
|
|
}
|