ank/RBE2002
1
Fork 0
Code Issues Activity

added accelration limit to motor controller

Browse source
This commit is contained in:
Andy Killorin 2024-10-25 11:03:10 -04:00
parent e38cb63c7f
commit 2463989237
Signed by: ank
GPG key ID: 23F9463ECB67FE8C
3 changed files with 29 additions and 18 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

17
lib/Chassis/src/chassis.h
View file

@ -2,25 +2,10 @@
#include <Arduino.h>
#include "event_timer.h"
#include "constants.h"
class Chassis
{
protected:
/**
* Kinematic parameters default to the spec sheet from Pololu. You'll need to fine
* tune them.
*/
const float ROBOT_RADIUS = 14.7 / 2;
const float LEFT_TICKS_PER_CM = 1440.0 / (3.1416 * 4.0);
const float RIGHT_TICKS_PER_CM = 1440.0 / (3.1416 * 4.0);
/**
* You can change the control loop period, but you should use multiples of 4 ms to
* avoid rounding errors.
*/
const uint16_t CONTROL_LOOP_PERIOD_MS = 20;
const float CONTROL_LOOP_PERIOD_S = CONTROL_LOOP_PERIOD_MS / 1000.;
protected:
/**
* loopFlag is used to tell the program when to update. It is set when Timer4

18
lib/Chassis/src/constants.h Normal file
View file

@ -0,0 +1,18 @@
#pragma once
#include <Arduino.h>
/**
* Kinematic parameters default to the spec sheet from Pololu. You'll need to fine
* tune them.
*/
const float ROBOT_RADIUS = 14.7 / 2;
const float LEFT_TICKS_PER_CM = 1440.0 / (3.1416 * 4.0);
const float RIGHT_TICKS_PER_CM = 1440.0 / (3.1416 * 4.0);
/**
* You can change the control loop period, but you should use multiples of 4 ms to
* avoid rounding errors.
*/
const uint16_t CONTROL_LOOP_PERIOD_MS = 20;
const float CONTROL_LOOP_PERIOD_S = CONTROL_LOOP_PERIOD_MS / 1000.;

12
lib/Romi32U4Motors/src/Romi32U4MotorTemplate.h
View file

@ -2,6 +2,7 @@
#include <Arduino.h>
#include <FastGPIO.h>
#include <constants.h>
// define the motor pins here
#define PWM_L 10
@ -33,8 +34,10 @@ protected:
// Used to keep track of the target speed, in counts / interval.
float targetSpeed = 0;
float TRAPEZOIDAL_RAMP_RATE_CM_S = 30.;
// maximum accelration in counts/interval
float TRAPEZOIDAL_RAMP_RATE = 0;
float TRAPEZOIDAL_RAMP_RATE = TRAPEZOIDAL_RAMP_RATE_CM_S / LEFT_TICKS_PER_CM / CONTROL_LOOP_PERIOD_S;
float currentSetpoint = 0;
/**
* This is the speed of the motor, in "encoder counts / encoder interval".
@ -124,7 +127,12 @@ protected:
* TODO: Implement integral, derivative control for the motors here!
*/
// Calculate the error in speed
float error = targetSpeed - speed;
float deltaSetpoint = targetSpeed - currentSetpoint;
deltaSetpoint = constrain(deltaSetpoint, -TRAPEZOIDAL_RAMP_RATE, TRAPEZOIDAL_RAMP_RATE);
currentSetpoint += deltaSetpoint;
float error = currentSetpoint - speed;
sumError += error;