kopia lustrzana https://github.com/pimoroni/pimoroni-pico
Indentation fixes
rodzic
898e4bace4
commit
24296af3d5
|
@ -61,9 +61,9 @@ int main() {
|
||||||
led.set_hsv(0, offset / 2.0f, 1.0f, BRIGHTNESS);
|
led.set_hsv(0, offset / 2.0f, 1.0f, BRIGHTNESS);
|
||||||
|
|
||||||
// Update all the motors
|
// Update all the motors
|
||||||
for(auto m = 0u; m < NUM_MOTORS; m++) {
|
for(auto m = 0u; m < NUM_MOTORS; m++) {
|
||||||
float angle = (((float)m / (float)NUM_MOTORS) + offset) * (float)M_PI;
|
float angle = (((float)m / (float)NUM_MOTORS) + offset) * (float)M_PI;
|
||||||
motors[m]->speed(sin(angle) * SPEED_EXTENT);
|
motors[m]->speed(sin(angle) * SPEED_EXTENT);
|
||||||
}
|
}
|
||||||
|
|
||||||
sleep_ms(1000 / UPDATES);
|
sleep_ms(1000 / UPDATES);
|
||||||
|
|
|
@ -101,20 +101,20 @@ int main() {
|
||||||
float percent_along = (float)update / (float)UPDATES_PER_MOVE;
|
float percent_along = (float)update / (float)UPDATES_PER_MOVE;
|
||||||
|
|
||||||
switch(INTERP_MODE) {
|
switch(INTERP_MODE) {
|
||||||
case 0:
|
case 0:
|
||||||
// Move the motor instantly to the end value
|
// Move the motor instantly to the end value
|
||||||
pos_pid.setpoint = end_value;
|
pos_pid.setpoint = end_value;
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case 2:
|
case 2:
|
||||||
// Move the motor between values using cosine
|
// Move the motor between values using cosine
|
||||||
pos_pid.setpoint = (((-cosf(percent_along * (float)M_PI) + 1.0) / 2.0) * (end_value - start_value)) + start_value;
|
pos_pid.setpoint = (((-cosf(percent_along * (float)M_PI) + 1.0) / 2.0) * (end_value - start_value)) + start_value;
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case 1:
|
case 1:
|
||||||
default:
|
default:
|
||||||
// Move the motor linearly between values
|
// Move the motor linearly between values
|
||||||
pos_pid.setpoint = (percent_along * (end_value - start_value)) + start_value;
|
pos_pid.setpoint = (percent_along * (end_value - start_value)) + start_value;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Calculate the velocity to move the motor closer to the position setpoint
|
// Calculate the velocity to move the motor closer to the position setpoint
|
||||||
|
@ -125,9 +125,9 @@ int main() {
|
||||||
|
|
||||||
// Print out the current motor values and their setpoints, but only on every multiple
|
// Print out the current motor values and their setpoints, but only on every multiple
|
||||||
if(print_count == 0) {
|
if(print_count == 0) {
|
||||||
printf("Pos = %f, ", capture.degrees());
|
printf("Pos = %f, ", capture.degrees());
|
||||||
printf("Pos SP = %f, ", pos_pid.setpoint);
|
printf("Pos SP = %f, ", pos_pid.setpoint);
|
||||||
printf("Speed = %f\n", m.speed() * SPD_PRINT_SCALE);
|
printf("Speed = %f\n", m.speed() * SPD_PRINT_SCALE);
|
||||||
}
|
}
|
||||||
|
|
||||||
// Increment the print count, and wrap it
|
// Increment the print count, and wrap it
|
||||||
|
@ -137,11 +137,11 @@ int main() {
|
||||||
|
|
||||||
// Have we reached the end of this movement?
|
// Have we reached the end of this movement?
|
||||||
if(update >= UPDATES_PER_MOVE) {
|
if(update >= UPDATES_PER_MOVE) {
|
||||||
update = 0; // Reset the counter
|
update = 0; // Reset the counter
|
||||||
|
|
||||||
// Set the start as the last end and create a new random end value
|
// Set the start as the last end and create a new random end value
|
||||||
start_value = end_value;
|
start_value = end_value;
|
||||||
end_value = (((float)rand() / (float)RAND_MAX) * (POSITION_EXTENT * 2.0f)) - POSITION_EXTENT;
|
end_value = (((float)rand() / (float)RAND_MAX) * (POSITION_EXTENT * 2.0f)) - POSITION_EXTENT;
|
||||||
}
|
}
|
||||||
|
|
||||||
sleep_ms(UPDATE_RATE * 1000.0f);
|
sleep_ms(UPDATE_RATE * 1000.0f);
|
||||||
|
|
|
@ -109,20 +109,20 @@ int main() {
|
||||||
float percent_along = (float)update / (float)UPDATES_PER_MOVE;
|
float percent_along = (float)update / (float)UPDATES_PER_MOVE;
|
||||||
|
|
||||||
switch(INTERP_MODE) {
|
switch(INTERP_MODE) {
|
||||||
case 0:
|
case 0:
|
||||||
// Move the motor instantly to the end value
|
// Move the motor instantly to the end value
|
||||||
pos_pid.setpoint = end_value;
|
pos_pid.setpoint = end_value;
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case 2:
|
case 2:
|
||||||
// Move the motor between values using cosine
|
// Move the motor between values using cosine
|
||||||
pos_pid.setpoint = (((-cosf(percent_along * (float)M_PI) + 1.0) / 2.0) * (end_value - start_value)) + start_value;
|
pos_pid.setpoint = (((-cosf(percent_along * (float)M_PI) + 1.0) / 2.0) * (end_value - start_value)) + start_value;
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case 1:
|
case 1:
|
||||||
default:
|
default:
|
||||||
// Move the motor linearly between values
|
// Move the motor linearly between values
|
||||||
pos_pid.setpoint = (percent_along * (end_value - start_value)) + start_value;
|
pos_pid.setpoint = (percent_along * (end_value - start_value)) + start_value;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Calculate the velocity to move the motor closer to the position setpoint
|
// Calculate the velocity to move the motor closer to the position setpoint
|
||||||
|
@ -154,11 +154,11 @@ int main() {
|
||||||
|
|
||||||
// Have we reached the end of this movement?
|
// Have we reached the end of this movement?
|
||||||
if(update >= UPDATES_PER_MOVE) {
|
if(update >= UPDATES_PER_MOVE) {
|
||||||
update = 0; // Reset the counter
|
update = 0; // Reset the counter
|
||||||
|
|
||||||
// Set the start as the last end and create a new random end value
|
// Set the start as the last end and create a new random end value
|
||||||
start_value = end_value;
|
start_value = end_value;
|
||||||
end_value = (((float)rand() / (float)RAND_MAX) * (POSITION_EXTENT * 2.0f)) - POSITION_EXTENT;
|
end_value = (((float)rand() / (float)RAND_MAX) * (POSITION_EXTENT * 2.0f)) - POSITION_EXTENT;
|
||||||
}
|
}
|
||||||
|
|
||||||
sleep_ms(UPDATE_RATE * 1000.0f);
|
sleep_ms(UPDATE_RATE * 1000.0f);
|
||||||
|
|
|
@ -120,12 +120,12 @@ int main() {
|
||||||
// Print out the current motor values and their setpoints,
|
// Print out the current motor values and their setpoints,
|
||||||
// but only for the first few updates and only every multiple
|
// but only for the first few updates and only every multiple
|
||||||
if(update < (uint)(PRINT_WINDOW * UPDATES) && print_count == 0) {
|
if(update < (uint)(PRINT_WINDOW * UPDATES) && print_count == 0) {
|
||||||
printf("Pos = %f, ", capture.degrees());
|
printf("Pos = %f, ", capture.degrees());
|
||||||
printf("Pos SP = %f, ", pos_pid.setpoint);
|
printf("Pos SP = %f, ", pos_pid.setpoint);
|
||||||
printf("Vel = %f, ", capture.revolutions_per_second() * SPD_PRINT_SCALE);
|
printf("Vel = %f, ", capture.revolutions_per_second() * SPD_PRINT_SCALE);
|
||||||
printf("Vel SP = %f, ", vel_pid.setpoint * SPD_PRINT_SCALE);
|
printf("Vel SP = %f, ", vel_pid.setpoint * SPD_PRINT_SCALE);
|
||||||
printf("Accel = %f, ", accel * ACC_PRINT_SCALE);
|
printf("Accel = %f, ", accel * ACC_PRINT_SCALE);
|
||||||
printf("Speed = %f\n", m.speed());
|
printf("Speed = %f\n", m.speed());
|
||||||
}
|
}
|
||||||
|
|
||||||
// Increment the print count, and wrap it
|
// Increment the print count, and wrap it
|
||||||
|
@ -135,10 +135,10 @@ int main() {
|
||||||
|
|
||||||
// Have we reached the end of this time window?
|
// Have we reached the end of this time window?
|
||||||
if(update >= (uint)(MOVEMENT_WINDOW * UPDATES)) {
|
if(update >= (uint)(MOVEMENT_WINDOW * UPDATES)) {
|
||||||
update = 0; // Reset the counter
|
update = 0; // Reset the counter
|
||||||
|
|
||||||
// Set the new position setpoint to be the inverse of the current setpoint
|
// Set the new position setpoint to be the inverse of the current setpoint
|
||||||
pos_pid.setpoint = 0.0 - pos_pid.setpoint;
|
pos_pid.setpoint = 0.0 - pos_pid.setpoint;
|
||||||
}
|
}
|
||||||
|
|
||||||
sleep_ms(UPDATE_RATE * 1000.0f);
|
sleep_ms(UPDATE_RATE * 1000.0f);
|
||||||
|
|
|
@ -105,9 +105,9 @@ int main() {
|
||||||
// Print out the current motor values and their setpoints,
|
// Print out the current motor values and their setpoints,
|
||||||
// but only for the first few updates and only every multiple
|
// but only for the first few updates and only every multiple
|
||||||
if(update < (uint)(PRINT_WINDOW * UPDATES) && print_count == 0) {
|
if(update < (uint)(PRINT_WINDOW * UPDATES) && print_count == 0) {
|
||||||
printf("Pos = %f, ", capture.degrees());
|
printf("Pos = %f, ", capture.degrees());
|
||||||
printf("Pos SP = %f, ", pos_pid.setpoint);
|
printf("Pos SP = %f, ", pos_pid.setpoint);
|
||||||
printf("Speed = %f\n", m.speed() * SPD_PRINT_SCALE);
|
printf("Speed = %f\n", m.speed() * SPD_PRINT_SCALE);
|
||||||
}
|
}
|
||||||
|
|
||||||
// Increment the print count, and wrap it
|
// Increment the print count, and wrap it
|
||||||
|
@ -117,10 +117,10 @@ int main() {
|
||||||
|
|
||||||
// Have we reached the end of this time window?
|
// Have we reached the end of this time window?
|
||||||
if(update >= (uint)(MOVEMENT_WINDOW * UPDATES)) {
|
if(update >= (uint)(MOVEMENT_WINDOW * UPDATES)) {
|
||||||
update = 0; // Reset the counter
|
update = 0; // Reset the counter
|
||||||
|
|
||||||
// Set the new position setpoint to be the inverse of the current setpoint
|
// Set the new position setpoint to be the inverse of the current setpoint
|
||||||
pos_pid.setpoint = 0.0f - pos_pid.setpoint;
|
pos_pid.setpoint = 0.0f - pos_pid.setpoint;
|
||||||
}
|
}
|
||||||
|
|
||||||
sleep_ms(UPDATE_RATE * 1000.0f);
|
sleep_ms(UPDATE_RATE * 1000.0f);
|
||||||
|
|
|
@ -145,12 +145,12 @@ int main() {
|
||||||
|
|
||||||
// Have we reached the end of this movement?
|
// Have we reached the end of this movement?
|
||||||
if(update >= UPDATES_PER_MOVE) {
|
if(update >= UPDATES_PER_MOVE) {
|
||||||
update = 0; // Reset the counter
|
update = 0; // Reset the counter
|
||||||
|
|
||||||
// Swap the start and end values
|
// Swap the start and end values
|
||||||
float temp = start_value;
|
float temp = start_value;
|
||||||
start_value = end_value;
|
start_value = end_value;
|
||||||
end_value = temp;
|
end_value = temp;
|
||||||
}
|
}
|
||||||
|
|
||||||
sleep_ms(UPDATE_RATE * 1000.0f);
|
sleep_ms(UPDATE_RATE * 1000.0f);
|
||||||
|
|
|
@ -163,40 +163,40 @@ int main() {
|
||||||
|
|
||||||
// Have we reached the end of this movement?
|
// Have we reached the end of this movement?
|
||||||
if(update >= UPDATES_PER_MOVE) {
|
if(update >= UPDATES_PER_MOVE) {
|
||||||
update = 0; // Reset the counter
|
update = 0; // Reset the counter
|
||||||
|
|
||||||
// Move on to the next part of the sequence
|
// Move on to the next part of the sequence
|
||||||
sequence += 1;
|
sequence += 1;
|
||||||
|
|
||||||
// Loop the sequence back around
|
// Loop the sequence back around
|
||||||
if(sequence >= 7) {
|
if(sequence >= 7) {
|
||||||
sequence = 0;
|
sequence = 0;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// Set the motor speeds, based on the sequence
|
// Set the motor speeds, based on the sequence
|
||||||
switch(sequence) {
|
switch(sequence) {
|
||||||
case 0:
|
case 0:
|
||||||
drive_forward(DRIVING_SPEED);
|
drive_forward(DRIVING_SPEED);
|
||||||
break;
|
break;
|
||||||
case 1:
|
case 1:
|
||||||
drive_forward(-DRIVING_SPEED);
|
drive_forward(-DRIVING_SPEED);
|
||||||
break;
|
break;
|
||||||
case 2:
|
case 2:
|
||||||
turn_right(DRIVING_SPEED);
|
turn_right(DRIVING_SPEED);
|
||||||
break;
|
break;
|
||||||
case 3:
|
case 3:
|
||||||
turn_right(-DRIVING_SPEED);
|
turn_right(-DRIVING_SPEED);
|
||||||
break;
|
break;
|
||||||
case 4:
|
case 4:
|
||||||
strafe_right(DRIVING_SPEED);
|
strafe_right(DRIVING_SPEED);
|
||||||
break;
|
break;
|
||||||
case 5:
|
case 5:
|
||||||
strafe_right(-DRIVING_SPEED);
|
strafe_right(-DRIVING_SPEED);
|
||||||
break;
|
break;
|
||||||
default:
|
default:
|
||||||
stop();
|
stop();
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
||||||
sleep_ms(UPDATE_RATE * 1000.0f);
|
sleep_ms(UPDATE_RATE * 1000.0f);
|
||||||
|
|
Ładowanie…
Reference in New Issue