122 lines
3.1 KiB
C++
122 lines
3.1 KiB
C++
#include <Arduino.h>
|
|
#include <Wire.h>
|
|
#include <smartmotor.h>
|
|
#include <SMC_gains.h>
|
|
#include <SMC_pid_directions.h>
|
|
#include "trapezoidal.h"
|
|
#include "constants.h"
|
|
|
|
SmartMotor left_motor(0x0A);
|
|
SmartMotor right_motor(0x0B);
|
|
|
|
enum ROBOT_STATE {
|
|
FORWARD,
|
|
TURN,
|
|
RETURN,
|
|
COMPLETE,
|
|
};
|
|
|
|
float CURRENT_POSITION = 0.0;
|
|
float CURRENT_ROTATION = 0.0;
|
|
struct Setpoint setpoint;
|
|
float velocity = 40.0;
|
|
// start of current state
|
|
float phase_start;
|
|
enum ROBOT_STATE robot_state;
|
|
|
|
struct Trapezoidal forward = {VEL_LIMIT, ACCEL_LIMIT, FORWARD_DISTANCE};
|
|
|
|
void write_rpm_ff(SmartMotor* motor, int32_t rpm, float ff) {
|
|
if (rpm == 0) {rpm = 1;};
|
|
float kV = ff / (float) rpm; // calculate velocity feedforward that causes desired absolute feedforward
|
|
motor->tune_vel_pid(kV, KP,KI,KD);
|
|
delay(1);
|
|
motor->write_rpm(rpm);
|
|
delay(1);
|
|
}
|
|
|
|
void setup() {
|
|
// INIT SERIAL
|
|
Serial.begin(115200);
|
|
while(!Serial);
|
|
|
|
Wire.begin(); // INIT ARDUINO UNO AS I2C CONTROLLER
|
|
|
|
// TUNE VELOCITY PID
|
|
//left_motor.tune_vel_pid(0.9, 3.7,0.3,0.0);
|
|
//delay(10);
|
|
//right_motor.tune_vel_pid(0.9, 3.7,0.3,0.0);
|
|
delay(10);
|
|
//right_motor.set_direction(PIDDirection::DIRECT);
|
|
|
|
phase_start = (float)millis() / 1000.0;
|
|
robot_state = FORWARD;
|
|
}
|
|
|
|
void loop() {
|
|
float time = (float)millis() / 1000.0 - phase_start;
|
|
switch (robot_state) {
|
|
case FORWARD:
|
|
setpoint = trapezoidal_planner(&forward, time);
|
|
break;
|
|
case TURN:
|
|
break;
|
|
case RETURN:
|
|
break;
|
|
}
|
|
|
|
float feedforward =
|
|
setpoint.acceleration * CMS_RPM * FF_ACCEL +
|
|
setpoint.velocity * CMS_RPM * FF_VEL +
|
|
((setpoint.velocity > 0.0) ? FF_STAT : -FF_STAT);
|
|
|
|
float feedforward_right =
|
|
setpoint.acceleration * CMS_RPM * 9.0 +
|
|
setpoint.velocity * CMS_RPM * FF_VEL +
|
|
((setpoint.velocity > 0.0) ? FF_STAT : -FF_STAT);
|
|
|
|
write_rpm_ff(&left_motor, setpoint.velocity * CMS_RPM, feedforward);
|
|
write_rpm_ff(&right_motor, (robot_state == TURN ? setpoint.velocity : -setpoint.velocity) * CMS_RPM, feedforward);
|
|
|
|
// READ MOTOR POSITION
|
|
int32_t rpm = -right_motor.read_rpm();
|
|
int32_t pos = -right_motor.read_angle();
|
|
int32_t error = setpoint.velocity * CMS_RPM - rpm;
|
|
Serial.print("ff:");
|
|
Serial.print(feedforward_right);
|
|
Serial.print(",time:");
|
|
Serial.print(time);
|
|
Serial.print(",distgoal:");
|
|
Serial.print(setpoint.position);
|
|
Serial.print(",dist:");
|
|
Serial.print(pos * DEG_CM);
|
|
Serial.print(",cms/s:");
|
|
Serial.print(rpm / CMS_RPM);
|
|
Serial.print(",setvel:");
|
|
Serial.print(setpoint.velocity);
|
|
Serial.print(",setacc:");
|
|
Serial.print(setpoint.acceleration);
|
|
Serial.print(",Err:");
|
|
Serial.print(error / CMS_RPM);
|
|
Serial.println("");
|
|
|
|
// move on if at setpoint TODO: check that the error is low as well
|
|
if (setpoint.complete) {
|
|
switch (robot_state) {
|
|
case FORWARD:
|
|
robot_state = TURN;
|
|
phase_start = (float)millis() / 1000.0;
|
|
|
|
// end (temp)
|
|
left_motor.write_rpm(0.0);
|
|
delay(10);
|
|
right_motor.write_rpm(0.0);
|
|
for (;;) {};
|
|
break;
|
|
case TURN:
|
|
break;
|
|
case RETURN:
|
|
break;
|
|
}
|
|
}
|
|
}
|