kopia lustrzana https://github.com/pimoroni/pimoroni-pico
127 wiersze
2.4 KiB
C++
127 wiersze
2.4 KiB
C++
#include <stdio.h>
|
|
#include "pico/stdlib.h"
|
|
|
|
#include "pico_motor_shim.hpp"
|
|
|
|
/*
|
|
An example of how to perform simple movements of a 2-wheeled driving robot.
|
|
*/
|
|
|
|
using namespace motor;
|
|
|
|
// The scaling to apply to each motor's speed to match its real-world speed
|
|
constexpr float SPEED_SCALE = 5.4f;
|
|
|
|
// The speed to drive the wheels at, from 0.0 to SPEED_SCALE
|
|
constexpr float DRIVING_SPEED = SPEED_SCALE;
|
|
|
|
|
|
// Create the left and right motors with a given speed scale
|
|
// Swap the numbers and directions if this is different to your setup
|
|
Motor left(pico_motor_shim::MOTOR_1, NORMAL_DIR, SPEED_SCALE);
|
|
Motor right(pico_motor_shim::MOTOR_2, REVERSED_DIR, SPEED_SCALE);
|
|
|
|
|
|
// Helper functions for driving in common directions
|
|
void forward(float speed=DRIVING_SPEED) {
|
|
left.speed(speed);
|
|
right.speed(speed);
|
|
}
|
|
|
|
void backward(float speed=DRIVING_SPEED) {
|
|
left.speed(-speed);
|
|
right.speed(-speed);
|
|
}
|
|
|
|
void turn_left(float speed=DRIVING_SPEED) {
|
|
left.speed(-speed);
|
|
right.speed(speed);
|
|
}
|
|
|
|
void turn_right(float speed=DRIVING_SPEED) {
|
|
left.speed(speed);
|
|
right.speed(-speed);
|
|
}
|
|
|
|
void curve_forward_left(float speed=DRIVING_SPEED) {
|
|
left.speed(0.0);
|
|
right.speed(speed);
|
|
}
|
|
|
|
void curve_forward_right(float speed=DRIVING_SPEED) {
|
|
left.speed(speed);
|
|
right.speed(0.0);
|
|
}
|
|
|
|
void curve_backward_left(float speed=DRIVING_SPEED) {
|
|
left.speed(0.0);
|
|
right.speed(-speed);
|
|
}
|
|
|
|
void curve_backward_right(float speed=DRIVING_SPEED) {
|
|
left.speed(-speed);
|
|
right.speed(0.0);
|
|
}
|
|
|
|
void stop() {
|
|
left.stop();
|
|
right.stop();
|
|
}
|
|
|
|
void coast() {
|
|
left.coast();
|
|
right.coast();
|
|
}
|
|
|
|
|
|
int main() {
|
|
stdio_init_all();
|
|
|
|
// Initialise the motors
|
|
if(!left.init() || !right.init()) {
|
|
printf("Cannot initialise the motors. Check the provided parameters\n");
|
|
return 0;
|
|
}
|
|
|
|
// Demo each of the move methods
|
|
forward();
|
|
sleep_ms(1000);
|
|
|
|
backward();
|
|
sleep_ms(1000);
|
|
|
|
curve_forward_right();
|
|
sleep_ms(1000);
|
|
|
|
curve_forward_left();
|
|
sleep_ms(1000);
|
|
|
|
turn_right();
|
|
sleep_ms(1000);
|
|
|
|
forward(0.5 * DRIVING_SPEED); // Half speed
|
|
sleep_ms(1000);
|
|
|
|
turn_left(0.5 * DRIVING_SPEED); // Half speed
|
|
sleep_ms(1000);
|
|
|
|
curve_backward_right(0.75 * DRIVING_SPEED); // Three quarters speed
|
|
sleep_ms(1000);
|
|
|
|
forward(); // Full speed
|
|
sleep_ms(500);
|
|
|
|
coast(); // Come to a halt gently
|
|
sleep_ms(1000);
|
|
|
|
forward();
|
|
sleep_ms(500);
|
|
|
|
stop(); // Apply the brakes
|
|
sleep_ms(1000);
|
|
|
|
// Disable the motors
|
|
left.disable();
|
|
right.disable();
|
|
}
|