150 lines
4.2 KiB
C++
150 lines
4.2 KiB
C++
#include <Arduino.h>
|
|
#include <Wire.h>
|
|
#include <smartmotor.h>
|
|
#include <SMC_gains.h>
|
|
|
|
const float ACCEL_LIMIT = 4.0; // cm/s/s
|
|
const float VEL_LIMIT = 16.0; // cm/s
|
|
const float BACKLASH_RIGHT = 0.0; // degrees
|
|
const float BACKLASH_LEFT = 0.0; // degrees
|
|
const float WHEEL_DIAMETER = 6.37; // cm
|
|
const float WHEEL_TO_WHEEL = 0.0; // cm
|
|
|
|
const float FORWARD_DISTANCE = 100.0; // cm
|
|
const float TURN_AMOUNT = 180.0; // degrees
|
|
const float RETURN_DISTANCE = 100.0; // cm
|
|
|
|
SmartMotor left_motor(0x0A);
|
|
SmartMotor right_motor(0x0B);
|
|
|
|
void setup() {
|
|
// INIT SERIAL
|
|
Serial.begin(115200);
|
|
while(!Serial);
|
|
|
|
Wire.begin(); // INIT ARDUINO UNO AS I2C CONTROLLER
|
|
|
|
// TUNE POSITION PID
|
|
left_motor.tune_vel_pid(9.0, 0.0,0.0,0.0);
|
|
right_motor.tune_vel_pid(1.0, 0.65,0.060,0.065);
|
|
delay(1000);
|
|
|
|
// SET MOTOR POSITION
|
|
//int32_t angle=360;
|
|
//uint8_t status= left_motor.write_angle(angle);
|
|
//delay(1000);
|
|
|
|
// READ MOTOR POSITION
|
|
int32_t pos = left_motor.read_angle();
|
|
Serial.print("Pos: ");
|
|
Serial.print(pos);
|
|
Serial.println(" deg");
|
|
}
|
|
|
|
enum ROBOT_STATE {
|
|
FORWARD,
|
|
TURN,
|
|
RETURN,
|
|
COMPLETE,
|
|
};
|
|
|
|
enum ROBOT_STATE robot_state = FORWARD;
|
|
|
|
float CURRENT_POSITION = 0.0;
|
|
float CURRENT_ROTATION = 0.0;
|
|
float velocity = 40.0;
|
|
|
|
struct Setpoint {
|
|
float velocity; // unitless
|
|
float position; // unitless
|
|
bool complete; // the setpoint will no longer change
|
|
};
|
|
|
|
// returns the position and velocity at the given time on a trapezoidal motion plan
|
|
// this could be baked if too computationally expensive
|
|
// not fully fuzzed
|
|
struct Setpoint trapezoidal_planner(float max_vel, float max_acc, float dist, float time) {
|
|
struct Setpoint setpoint = {0.0, 0.0};
|
|
|
|
float time_accelerating = max_acc / max_vel;
|
|
float distance_while_accelerating = 0.5 * max_acc * time_accelerating * time_accelerating;
|
|
|
|
if (2.0 * distance_while_accelerating > dist) {
|
|
// triangular
|
|
float peak_velocity_time = sqrt(dist/max_acc);
|
|
float peak_velocity = max_acc * peak_velocity_time;
|
|
|
|
if (time < peak_velocity_time) {
|
|
// accelerating
|
|
setpoint.velocity = max_acc * time;
|
|
setpoint.position = 0.5 * max_acc * time * time;
|
|
} else if (time < peak_velocity_time * 2.0) {
|
|
// slowing down
|
|
float time_decay = time - peak_velocity_time;
|
|
setpoint.velocity = peak_velocity - (time_decay * max_acc);
|
|
setpoint.position = (0.5 * max_acc * peak_velocity_time * peak_velocity_time) // acceleration phase
|
|
+ peak_velocity * time_decay
|
|
- 0.5 * max_acc * time_decay * time_decay;
|
|
} else {
|
|
setpoint.velocity = 0;
|
|
setpoint.position = dist;
|
|
setpoint.complete = true;
|
|
}
|
|
} else {
|
|
// trapezoidal
|
|
float cruise_distance = dist - 2.0 * distance_while_accelerating;
|
|
float cruise_time = cruise_distance / max_vel;
|
|
float total_time = 2 * time_accelerating + cruise_time;
|
|
if (time < time_accelerating) {
|
|
// still accelerating
|
|
setpoint.velocity = time * max_acc;
|
|
setpoint.position = 0.5 * max_acc * time * time;
|
|
} else if (time < time_accelerating + cruise_time) {
|
|
// cruising
|
|
setpoint.velocity = max_vel;
|
|
setpoint.position = distance_while_accelerating + max_vel * (time - time_accelerating);
|
|
} else if (time < total_time) {
|
|
// slowing down
|
|
float time_decay = time - (time_accelerating + cruise_time);
|
|
setpoint.velocity = max_vel - time_decay * max_acc;
|
|
setpoint.position = distance_while_accelerating + cruise_distance
|
|
+ (max_vel * time_decay)
|
|
- 0.5 * max_acc * time_decay * time_decay;
|
|
} else {
|
|
//done
|
|
setpoint.velocity = 0.0;
|
|
setpoint.position = dist;
|
|
setpoint.complete = true;
|
|
}
|
|
}
|
|
|
|
return setpoint;
|
|
}
|
|
|
|
void loop() {
|
|
switch (robot_state) {
|
|
case FORWARD:
|
|
break;
|
|
case TURN:
|
|
break;
|
|
case RETURN:
|
|
break;
|
|
}
|
|
|
|
|
|
|
|
left_motor.write_rpm(velocity);
|
|
//right_motor.write_rpm(robot_state == TURN ? velocity : -velocity);
|
|
delay(20);
|
|
|
|
// READ MOTOR POSITION
|
|
int32_t rpm = left_motor.read_rpm();
|
|
int32_t error = velocity - rpm;
|
|
Serial.print("RPM: ");
|
|
Serial.print(rpm);
|
|
Serial.print(" Setpoint: ");
|
|
Serial.print(velocity);
|
|
Serial.print(" Err: ");
|
|
Serial.print(error);
|
|
Serial.println("");
|
|
}
|