Got it working! Thanks for all the help from this thread. I’m amazed how quickly it came together. You can see in the video how well it works.
Examples:
G30 to move to position 30 (rads)
GV100 to change velocity to 100 (rads/s)
GA5 to change acceleration to 5 (rads/s/s)
Here are the 2 files I added to the Dagor controller’s code base to get the G command added into the SimpleFOC commander. Keep in mind this is very messy code. Lots of globals. Not well contained into a struct. If you issue a new Gcode move in the middle of an existing one the behavior is weird. However, this is an amazing start. Would love help from the community to refine the code.
This is the main file that’s as close to a port of the Python script I posted above as I could get it on quick order.
ACCELDECEL_PLANNER.ino
// Acceleration / Deceleration Planner for SimpleFOC & Dagor Controller
// Copyright (c) 2021 John Lauer for ACCELDECEL_PLANNER.ino
// This library allows you to control SimpleFOC with a plan for moves at a certain velocity as well
// as acceleration and deceleration at a certain rate. This code follows the same license as the original code
// given credit to below.
//
// Credit for most of this concept and much of the code used as a starting point for this library goes to
// the code inside ODrive available at:
// https://github.com/odriverobotics/ODrive/blob/a8461c40fd6d97991adc9d553db3fdbce4a01dc4/tools/motion_planning/PlanTrap.py
//
// # Original copyrights and code by
// # Copyright (c) 2018 Paul Guénette
// # Copyright (c) 2018 Oskar Weigl
// # Permission is hereby granted, free of charge, to any person obtaining a copy
// # of this software and associated documentation files (the "Software"), to deal
// # in the Software without restriction, including without limitation the rights
// # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// # copies of the Software, and to permit persons to whom the Software is
// # furnished to do so, subject to the following conditions:
// # The above copyright notice and this permission notice shall be included in all
// # copies or substantial portions of the Software.
// # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// # SOFTWARE.
// # This algorithm is based on:
// # FIR filter-based online jerk-constrained trajectory generation
// # https://www.researchgate.net/profile/Richard_Bearee/publication/304358769_FIR_filter-based_online_jerk-controlled_trajectory_generation/links/5770ccdd08ae10de639c0ff7/FIR-filter-based-online-jerk-controlled-trajectory-generation.pdf
// Symbol Description
// Ta, Tv and Td Duration of the stages of the AL profile
// Xi and Vi Adapted initial conditions for the AL profile
// Xf Position set-point
// s Direction (sign) of the trajectory
// Vmax, Amax, Dmax and jmax Kinematic bounds
// Ar, Dr and Vr Reached values of acceleration and velocity
// float Xf = 30; // # Position we're moving to (rads)
// float Xi = 0; // # Initial position (rads)
// float Vi = 0; // # Initial velocity (rads/s)
float Vmax_ = 1000; //10; // # Velocity max (rads/s)
float Amax_ = 20; //5; // # Acceleration max (rads/s/s)
float Dmax_ = Amax_; // # Decelerations max (rads/s/s)
//struct Step_t {
float Y_;
float Yd_;
float Ydd_;
//};
// Private variables
float Tf_, Xi_, Xf_, Vi_, Ar_, Dr_, Vr_, Ta_, Td_, Tv_, yAccel_;
float sign(float val) {
if (val < 0) return -1.0f;
if (val == 0) return 0.0f;
//if val > 0:
return 1.0f;
}
// A sign function where input 0 has positive sign (not 0)
float sign_hard(float val) {
if (val < 0) return -1.0f;
return 1.0f;
}
// This method should get called at the start of a move to generate all of the global variables
// for the duration of the move. These numbers will be used each time you call calc_step_pos_at_arbitrary_time()
// during the loop() because of course you would not want to calc these globals each time.
bool calc_plan_trapezoidal_path_at_start(float Xf, float Xi, float Vi, float Vmax, float Amax, float Dmax) {
float dX = Xf - Xi; // Distance to travel
float stop_dist = (Vi * Vi) / (2.0f * Dmax); // Minimum stopping distance
float dXstop = sign(Vi) * stop_dist; // Minimum stopping displacement
float s = sign_hard(dX - dXstop); // Sign of coast velocity (if any)
Ar_ = s * Amax; // Maximum Acceleration (signed)
Dr_ = -s * Dmax; // Maximum Deceleration (signed)
Vr_ = s * Vmax; // Maximum Velocity (signed)
// If we start with a speed faster than cruising, then we need to decel instead of accel
// aka "double deceleration move" in the paper
if ((s * Vi) > (s * Vr_)) {
Ar_ = -s * Amax;
}
// Time to accel/decel to/from Vr (cruise speed)
Ta_ = (Vr_ - Vi) / Ar_;
Td_ = -Vr_ / Dr_;
// Integral of velocity ramps over the full accel and decel times to get
// minimum displacement required to reach cuising speed
float dXmin = 0.5f*Ta_*(Vr_ + Vi) + 0.5f*Td_*Vr_;
// Are we displacing enough to reach cruising speed?
if (s*dX < s*dXmin) {
// Short move (triangle profile)
Vr_ = s * sqrt((Dr_*sq(Vi) + 2*Ar_*Dr_*dX) / (Dr_ - Ar_));
Ta_ = max(0.0f, (Vr_ - Vi) / Ar_);
Td_ = max(0.0f, -Vr_ / Dr_);
Tv_ = 0.0f;
} else {
// Long move (trapezoidal profile)
Tv_ = (dX - dXmin) / Vr_;
}
// Fill in the rest of the values used at evaluation-time
Tf_ = Ta_ + Tv_ + Td_;
Xi_ = Xi;
Xf_ = Xf;
Vi_ = Vi;
yAccel_ = Xi + Vi*Ta_ + 0.5f*Ar_*sq(Ta_); // pos at end of accel phase
return true;
}
// # Pass in t as the current time
// # t would be calculated based on milliseconds() passed
// # since the start of the current move
void calc_step_pos_at_arbitrary_time(float t) {
//Step_t trajStep;
if (t < 0.0f) { // Initial Condition
Y_ = Xi_;
Yd_ = Vi_;
Ydd_ = 0.0f;
} else if (t < Ta_) { // Accelerating
Y_ = Xi_ + Vi_*t + 0.5f*Ar_*sq(t);
Yd_ = Vi_ + Ar_*t;
Ydd_ = Ar_;
} else if (t < Ta_ + Tv_) { // Coasting
Y_ = yAccel_ + Vr_*(t - Ta_);
Yd_ = Vr_;
Ydd_ = 0.0f;
} else if (t < Tf_) { // Deceleration
float td = t - Tf_;
Y_ = Xf_ + 0.5f*Dr_*sq(td);
Yd_ = Dr_*td;
Ydd_ = Dr_;
} else if (t >= Tf_) { // Final Condition
Y_ = Xf_;
Yd_ = 0.0f;
Ydd_ = 0.0f;
} else {
// TODO: report error here
}
return; // trajStep;
}
And then a utilities file that injects the commander command, parses it, and has the tick method called from the main loop.
g_UTILITIES.ino
//###########################################
// Utilities
//
// These are extra commands available to help
// use the Dagor controller. They register themselves into the commander
// so when you send ? to this controller in the Serial port, you can
// see these available utilities.
//###########################################
// This init is called from the setup step on boot of ESP32
// Add any registrations of utilities or commander commands here
void util_init() {
// Add command to commander for torque test. Do this via
// it's init() method.
// torque_test_init();
// Add command to commander for restart
command.add('r', util_restart_cb, "Restart the ESP32");
// Add command to commander for break in
command.add('b', util_breakin_cb, "Break in the motor");
// Add command to commander for stat
command.add('S', util_stats_cb, "Stats");
// Enable toggle
// command.add('e', util_enable_cb, "Enable/disable toggle");
// AccelDecel Planner
command.add('G', util_gcode_move_cb, "Gcode move Gxx, GVxx, or GAxx - Example: G30 moves to position in rads. GV10 sets velocity to 10 rads/s. GA5 sets acceleration to 5 rads/s/s.");
}
// 5 seconds later startup. Add your startup method here. It is called well after all other systems are booted.
void util_startup() {
// For now, actually run break in so can do this without serial port attached
//util_breakin_cb("");
// torque_test_cb("");
// Start the Gcode utility
//util_gcode_startup();
}
// Add your tick method here for your utility. This is called at 5hz.
bool util_is_did_startup_method = false;
byte util_startup_tick_ctr = 0;
void util_tick() {
if (util_is_did_startup_method == false) {
// we have not run our startup routine yet. run it after 10 ticks.
util_startup_tick_ctr++;
if (util_startup_tick_ctr >= 20) {
util_is_did_startup_method = true;
util_startup_tick_ctr = 0;
Serial.println("");
util_startup();
} else {
if (util_startup_tick_ctr == 1) {
Serial.print("Waiting to start utils...");
} else {
Serial.print(".");
}
}
}
}
// Add your slow tick method here for your utility. This is called at 5hz,
// so use somewhat sparingly.
void util_5hz_tick() {
// call our own tick method for util startup processing
util_tick();
// call your tick method here. will get called at 5hz.
//breakin_tick();
// torque_tick();
// this is the slow tick for repeating the loop over and over for debug of arm
util_gcode_loop_tick();
}
// Add your super fast tick method here for your utility. This is called as fast as possible,
// so use sparingly.
void util_fast_tick() {
// util_breakin_micro_tick();
util_gcode_tick();
}
// Prints out the delta in a nice format of the milliseconds since start to now
// t is time in seconds = millis()/1000;
void timeToString(char* string, size_t size)
{
unsigned long nowMillis = millis();
unsigned long seconds = nowMillis / 1000;
int days = seconds / 86400;
seconds %= 86400;
byte hours = seconds / 3600;
seconds %= 3600;
byte minutes = seconds / 60;
seconds %= 60;
snprintf(string, size, "%04d:%02d:%02d:%02d", days, hours, minutes, seconds);
}
// ---------------------
// AccelDecel Planner
// Gcode Move Gxx Vxx Axx
// Example: G30 V10 A5
// ----------------------
void util_gcode_move_cb(char* cbArg) {
//Serial.print("Gcode move. cbArg:");
//Serial.println(cbArg);
// Parse this string for vals
if (cbArg[0] == 'V') {
String str = String(cbArg);
// Remove v so can convert to a float
str = str.substring(1);
// Serial.print("As string removing the v:");
// Serial.println(str);
// Now that we have clean string, convert it to a float and set velocity max global variable
Vmax_ = str.toFloat();
Serial.print("User wants velocity change. Vmax_: ");
Serial.println(Vmax_);
// Try calling the planner to use this new velocity value
// We have to use the current pos, vel, and accel
// calc_plan_trapezoidal_path_at_start(Xf_, Xi_, Vi_, Vmax_, Amax_, Dmax_);
calc_plan_trapezoidal_path_at_start(Xf_, motor.shaft_angle, motor.shaft_velocity, Vmax_, Amax_, Dmax_);
}
else if (cbArg[0] == 'A') {
String str = String(cbArg);
// Remove a so can convert to a float
str = str.substring(1);
// Serial.print("As string removing the a:");
// Serial.println(str);
// Now that we have clean string, convert it to a float and set velocity max global variable
Amax_ = str.toFloat();
Dmax_ = Amax_; // For now force decel to be same as accel. At some point add extra command to allow either/or to be set.
Serial.print("User wants acceleration change. Amax_: ");
Serial.println(Amax_);
// Try calling the planner to use this new acceleration value
// calc_plan_trapezoidal_path_at_start(Xf_, Xi_, Vi_, Vmax_, Amax_, Dmax_);
calc_plan_trapezoidal_path_at_start(Xf_, motor.shaft_angle, motor.shaft_velocity, Vmax_, Amax_, Dmax_);
}
else if (cbArg[0] == 'L') {
// Loop mode
// This let's you just loop back and forth from one position to another
util_gcode_loop_start();
}
else {
// Presume they asked for a position move where the cbArg is just a number for the new position
String str = String(cbArg);
float newPosition = str.toFloat();
Serial.print("Move to new position (rads): ");
Serial.println(newPosition);
// Start the whole Gcode move process
util_gcode_moveto(newPosition);
}
}
// This is called by the util startup method that triggers about 5 seconds after the actuator boots up
float util_gcode_loop_posA = 20;
void util_gcode_startup() {
// Serial.println("Started up AccelDecel Planner Gcode moves");
Serial.println("Entering endless loop of Gcode moves");
Vmax_ = 1000;
Amax_ = 10;
util_gcode_loop_start();
}
bool is_util_gcode_moving = false;
// -1 off
// 0 want to get going to position A but idle
// 1 going to position A
// 2 want to get going to position B (startPos) but idle
// 3 going to position B (startPos)
//
int util_gcode_loop_status = -1;
void util_gcode_loop_start() {
// tell the status variable that we want to get going to position A
util_gcode_loop_status = 0;
// now, next time into the tick() method we'll trigger the moveto
}
void util_gcode_loop_end() {
// tell the status variable that we want to do nothing
// that means each time into tick nothing happens, the move will eventually finish, all is good
util_gcode_loop_status = -1;
// now, next time into the tick() method we'll trigger nothing
}
void util_gcode_loop_tick() {
if (util_gcode_loop_status == -1) {
// user does not want it on, so exit
return;
}
else if (util_gcode_loop_status == 0) {
// they want to get going to posA
util_gcode_moveto(util_gcode_loop_posA);
util_gcode_loop_status = 1; // so next time into tick we know we're executing our move to posA
}
else if (util_gcode_loop_status == 1) {
// we do nothing here while the move is going
// if the move is done, though, we start moving back to posB (startPos)
if (is_util_gcode_moving) {
// they are still moving, do nothing
return;
}
else {
// they must be done moving,
util_gcode_loop_status = 2;
}
}
else if (util_gcode_loop_status == 2) {
// they want to get going to posA
util_gcode_moveto(0);
util_gcode_loop_status = 3; // so next time into tick we know we're executing our move to posA
}
else if (util_gcode_loop_status == 3) {
// we do nothing here while the move is going
// if the move is done, though, we start moving back to posA (endPos)
if (is_util_gcode_moving) {
// they are still moving, do nothing
return;
}
else {
// they must be done moving,
util_gcode_loop_status = 0;
}
}
}
float new_pos_ = 0;
static unsigned long util_gcode_start_move_millis = 0;
// When ready to move to new position, call this with the new position
void util_gcode_moveto(float newPos) {
// See if already moving. Exit if so.
// if (is_util_gcode_moving) {
// Serial.println("Already in a move. Cancelling request to move to new position.");
// return;
// }
// set our global of the new position
Xf_ = newPos;
// set our milliseconds passed variable so we have a t to work with
util_gcode_start_move_millis = millis();
// take the position from SimpleFOC and set it to our start position
Xi_ = motor.shaft_angle;
Serial.print("Starting at shaft angle of:");
Serial.println(Xi_);
// TODO: If we are being asked to move but are already in motion, we should go with the current velocity, position, and acceleration
// and keep moving.
// Now we need to do our calcs before we start moving
// TODO: Maybe stop passing vars and just use all globals
calc_plan_trapezoidal_path_at_start(Xf_, Xi_, Vi_, Vmax_, Amax_, Dmax_);
motor.target = Y_; // could possibly put this in the method above
// Tell user how long this move will take
Serial.print("Time to complete move (secs):");
Serial.println(Tf_);
// Velocity and Accel of move
Serial.print("Velocity for move: ");
Serial.print(Vmax_);
Serial.print(", Acceleration: ");
Serial.println(Amax_);
// set our global bool so the tick method knows we have a move in motion
is_util_gcode_moving = true;
// motor.monitor_downsample = 250; // enable monitor every 250ms
}
// This should get call at 100hz
unsigned int gcode_step_ctr = 0;
static unsigned long lastTimeUpdateMillis = 0;
int util_gcode_period = 10; // 1000 / this number = Hz, i.e. 1000 / 100 = 10Hz, 1000 / 10 = 100Hz, 1000 / 5 = 200Hz, 1000 / 1 = 1000hZ
unsigned long util_gcode_time_now = 0;
void util_gcode_tick() {
// This should get entered 100 times each second (100Hz)
if((unsigned long)(millis() - util_gcode_time_now) > util_gcode_period){
util_gcode_time_now = millis();
//Serial.print(".");
// see if we are in a move or not
if (is_util_gcode_moving) {
// we are in a move, let's calc the next position
float secs_since_start_of_move = (millis() - util_gcode_start_move_millis) / 1000.0f;
//Serial.println(millis_since_start_of_move);
calc_step_pos_at_arbitrary_time(secs_since_start_of_move);
motor.target = Y_;
// Serial.print("t (secs): ");
// Serial.print(secs_since_start_of_move);
// Serial.print(", Y_ (rads): ");
// Serial.println(Y_);
// Called 4 times per second
/**
gcode_step_ctr++;
if (gcode_step_ctr >= 50) {
Serial.print(secs_since_start_of_move);
Serial.print(", ");
util_stats();
gcode_step_ctr = 0;
}
**/
// see if we are done with our move
if (secs_since_start_of_move >= Tf_) {
// we are done with move
// motor.monitor_downsample = 0; // disable monitor
Serial.println("Done with move");
is_util_gcode_moving = false;
}
}
// // Called once/second
// gcode_step_ctr++;
// if (gcode_step_ctr >= 25) {
// char str[20] = "";
// timeToString(str, sizeof(str));
// Serial.print(str);
// gcode_step_ctr = 0;
// util_stats();
// }
}
}
// ----------------
// ENABLE TOGGLE
// ----------------
void util_enable_cb(char* cbArg) {
Serial.print("Toggling enable. cbArg:");
Serial.println(cbArg);
util_enable_toggle();
}
void util_enable_toggle() {
int gate_val = digitalRead(enGate);
if (gate_val == HIGH) {
digitalWrite(enGate, LOW);
Serial.println("Toggled enable to off.");
} else {
digitalWrite(enGate, HIGH);
Serial.println("Toggled enable to on.");
}
}
// --------------
// RESTART
// ----------------
// The callback when user enters command at serial port. This restarts the ESP32.
void util_restart_cb(char* cbArg) {
Serial.print("Restarting ESP32. cbArg:");
Serial.println(cbArg);
ESP.restart();
}
// --------------
// BREAK IN MOTOR
// ----------------
bool breakin_is_running = false;
unsigned int breakin_tick_ctr = 0;
int breakin_vel_current = 0;
int breakin_vel_start = 1;
int breakin_vel_increment = 1;
int breakin_vel_end = 65; //125;
byte breakin_phase = 0; // 0:stop, 1:fwd accel to end, 2:fwd decel back to 0, 3:rev accel to -end, 4: rev decel back to 0
unsigned int util_breakin_stateT = 0;
// The callback when user enters command at serial port. This triggers the break in function.
void util_breakin_cb(char* cbArg) {
Serial.print("Toggling break in of motor. cbArg:");
Serial.print(cbArg);
if (breakin_is_running) {
Serial.println("Toggling OFF");
breakin_finish();
}
else {
//Serial.println("Starting break in of motor.");
Serial.println("Toggling ON");
Serial.println("We will rotate at differing velocities and in different directions");
Serial.println("in an unending loop. Enter 'b' again to toggle the break in off.");
Serial.print("We will accel/decel to +/- velocity of: ");
Serial.println(breakin_vel_end);
Serial.print("At a step increment of: ");
Serial.println(breakin_vel_increment);
breakin_begin();
}
}
void breakin_begin() {
// ok, now do move where we go to start position, pause for 2 seconds, then move back
breakin_tick_ctr = 0;
breakin_is_running = true;
//breakin_fwd_dir = true;
breakin_phase = 1; // 1:fwd accel to end
// make sure we are in velocity mode
breakin_vel_current = breakin_vel_start; // + torque_increment;
motor.target = breakin_vel_current;
motor.move();
//Serial.print("Setting velocity to: ");
//Serial.println(motor.target);
//Serial.println("The micro tick callback will execute at 25 hertz");
Serial.println("Going to phase 1:fwd accel to end");
util_stats();
}
void breakin_step() {
// increment velocity
breakin_vel_current = breakin_vel_current + breakin_vel_increment;
// see what phase we're in
// 0:stop, 1:fwd accel to end, 2:fwd decel to 0, then rev accel to -end, 3:rev decel back to 0
if (breakin_phase == 1) {
// see if we're at end
if (breakin_vel_current > breakin_vel_end) {
// if we get here, we're at end of forward direction, so now we need to go reverse direction
// meaning slow down
breakin_vel_increment = -1 * breakin_vel_increment;
breakin_vel_end = -1 * breakin_vel_end;
breakin_phase = 2; // skip to 2:rev accel to -end
Serial.println("Going to phase 2:fwd decel to 0, then rev accel to -end");
util_stats();
} else {
// i think do nothing for now
}
}
else if (breakin_phase == 2) {
// see if we're at -end
if (breakin_vel_current < breakin_vel_end) {
// if we get here, we're at end of reverse direction to -end, so now we need to go fwd direction
// meaning slow down back to 0
breakin_vel_increment = -1 * breakin_vel_increment;
breakin_vel_end = -1 * breakin_vel_end;
breakin_phase = 3;
Serial.println("Going to phase 3:rev decel back to 0");
util_stats();
} else {
// i think do nothing for now
}
}
else if (breakin_phase == 3) {
if (breakin_vel_current > 0) {
// This means we slowed all the way down to 0 so can end
Serial.println("Going back to phase 1.");
util_stats();
// Let's start all over again
breakin_begin();
//breakin_finish();
return;
}
}
else {
Serial.print("Got phase that should not get. breakin_phase:");
Serial.println(breakin_phase);
}
motor.target = breakin_vel_current;
motor.move();
//Serial.println("Going to new velocity for break in of motor.");
//Serial.print("Setting velocity to: ");
//Serial.println(motor.target);
}
void breakin_finish() {
// done with breakin, so set bool that we're done
breakin_is_running = false;
breakin_vel_increment = abs(breakin_vel_increment);
breakin_vel_end = abs(breakin_vel_end);
breakin_phase = 0;
breakin_vel_current = -1;
motor.target = 0;
motor.move();
Serial.println("Ending break in of motor.");
//Serial.print("Moving to: ");
//Serial.println(motor.target);
util_stats();
}
void util_breakin_micro_tick() {
util_breakin_stateT++;
// Functions inside this "if" will execute at a 50hz rate (the denominator)
if(util_breakin_stateT >= 100000/50){
util_breakin_stateT = 0;
breakin_tick();
//breakin_step();
}
}
// this is called at 5hz, that way we can trigger off of it
void breakin_tick() {
if (breakin_is_running) {
// At 2 seconds
//if (breakin_tick_ctr == 10) {
// torque_tick_ctr++;
// torque_relax();
//}
// Each time in here (50Hz)
breakin_step();
// At 1 second
if (breakin_tick_ctr >= 10) {
//Serial.println("breakin_tick_ctr was 15, so doing next step...");
breakin_tick_ctr = 0;
util_stats();
}
else {
breakin_tick_ctr++;
}
}
}
// --------------
// STATS
// ----------------
/**
// current target value
float target;
// current motor angle
float shaft_angle;
// current motor velocity
float shaft_velocity;
// current target velocity
float shaft_velocity_sp;
// current target angle
float shaft_angle_sp;
// current voltage set to the motor (voltage.q, voltage.d)
DQVoltage_s voltage;
// current (if) measured on the motor (current.q, current.d)
DQCurrent_s current;
// phase voltages
float Ua, Ub, Uc;
**/
void util_stats() {
Serial.print("Angle: ");
Serial.print(motor.shaft_angle);
Serial.print(", Velocity: ");
Serial.print(motor.shaft_velocity);
Serial.print(", Voltage q: ");
Serial.print(motor.voltage.q);
Serial.print(", Temp: ");
Serial.println(util_temp(), 2);
}
// The callback when user enters command at serial port. This restarts the ESP32.
void util_stats_cb(char* cbArg) {
//Serial.print("Stats. cbArg:");
//Serial.println(cbArg);
util_stats();
}
float util_temp() {
float vOut = 0;
for (byte ctr = 0; ctr < 10; ctr++) {
vOut += analogRead(vTemp);
}
return ((((vOut/10)*3.3)/4095)-1.8577)/-0.01177;
}
// --------------
// TORQUE TEST
// ----------------
// Setup torque test
float torque_pre_start = 0;
float torque_start = 10;
float torque_end = 50;
float torque_increment = 5;
float torque_current = -1; // stores the current move to target
bool torque_is_testing = false;
int torque_tick_ctr = 0;
void torque_test_init() {
// add command to commander so we can do torque test
command.add('t', torque_test_cb, "Torque Test");
}
// The callback when user enters command at serial port. This starts test.
void torque_test_cb(char* cbArg) {
Serial.print("torque_test_cb. cbArg:");
Serial.println(cbArg);
torque_begin();
}
void torque_begin() {
// ok, now do move where we go to start position, pause for 2 seconds, then move back
torque_tick_ctr = 0;
torque_is_testing = true;
// jerk control using voltage voltage ramp
// default value is 300 volts per sec ~ 0.3V per millisecond
//motor.PID_velocity.output_ramp = 1;
// setting the limits
// maximal velocity of the position control
motor.velocity_limit = 0.001; // rad/s - default 20
// acceleration control using output ramp
// this variable is in rad/s^2 and sets the limit of acceleration
motor.P_angle.output_ramp = 10; // default 1e6 rad/s^2
// angle P controller - default P=20
motor.P_angle.P = 0.5;
torque_current = torque_start; // + torque_increment;
motor.target = torque_current;
motor.move();
Serial.println("Starting torque test.");
Serial.print("Moving to: ");
Serial.println(motor.target);
}
// in this step we go back to the start number quickly, like after 2 ticks
// so that we don't put too much pressure on the mosfets or current draw
void torque_relax() {
motor.target = torque_pre_start;
motor.move();
Serial.println("Back to relax position.");
Serial.print("Moving to: ");
Serial.println(motor.target);
}
void torque_step() {
torque_current = torque_current + torque_increment;
// see if we're at end
if (torque_current > torque_end) {
torque_finish();
return;
}
motor.target = torque_current;
motor.move();
Serial.println("Next step on torque test.");
Serial.print("Moving to: ");
Serial.println(motor.target);
}
void torque_finish() {
// done testing at 2 seconds, so go back to start position
torque_is_testing = false;
torque_current = -1;
motor.target = torque_pre_start;
motor.move();
Serial.println("Ending torque test.");
Serial.print("Moving to: ");
Serial.println(motor.target);
// jerk control using voltage voltage ramp
// default value is 300 volts per sec ~ 0.3V per millisecond
//motor.PID_velocity.output_ramp = 1000;
// setting the limits
// maximal velocity of the position control
//motor.velocity_limit = 20; // rad/s - default 20
torque_begin();
}
// this is called at 5hz, that way we can trigger off of it
void torque_tick() {
if (torque_is_testing) {
if (torque_tick_ctr == 50) {
torque_tick_ctr++;
torque_relax();
}
else if (torque_tick_ctr == 100) {
Serial.println("torque_tick_ctr was 15, so doing next step...");
torque_tick_ctr = 0;
torque_step();
}
else {
torque_tick_ctr++;
}
}
}
Enjoy.