Soft Motion Motor Control & Trajectory Planninginterim Report
Soft Motion Motor Control & Trajectory PlanningInterim Report
By: Nicholas Curry
April 2, Spring 2017
Instructor: Dr. Cris Koutsougeras
Advisor: Mohammad Saadeh
Abstract:
The goal of this project is to operate a motor with precision and control while minimizing the effect of jerk. The effects of jerk can cause damage and vibration not needed in the system. Jerk is affectively neutralized by creating a 5rd order polynomial for the trajectory of a motor control system. The soft easy in and out of the start and end of the desired displacement causes the affect known as “soft motion” using a specific trajectory laid out in Matlab. The intent is to carefully craft a software that can produce soft motion if a usergives the desired parameters (total displacement per time OR displacement and max desired velocity) thus allowing the hardware of the motor to follow the planned trajectory.
Introduction:
Soft motion has many applications in the real world. The control system that is devised can be implemented in many different scenarios. For example, jerk needs to be limited in operations that have precious cargo, such as elevators or sky lifts with people, conveyer belts with sensitive or breakable objects, and robots of all types. These displacement objects are always high torque and depended on the speed taken from point A to point B. However, the important parts in not always the starting and ending points but rather the speed trajectory that is followed. For instance, I need to travel from one end of a room to the other, and there is a table in the middle of the room. Sometimes, the shortest path can cause harm to the traveler.
Motor control and displacement of objects can be though about in the same way. Many systems need motors to move mechanismswhile applying soft motion. That can help improve the performance on a system, thus causing less harm to the objects and tools.
The first test in this project will be the stepper motors control. Using the sine wave shown in degree below, was used in the Arduino code to simulate a 3rd order polynomial trajectory. By segmenting the sine wave from -90 to 270 degrees with a counter stalling at max speed for some unspecified set time. The final trajectory needed for this system will be developed for testing purposes and proof of concept. Not much time will be spent in this area.
Parts:
Motor STP-MTRH-34127: (SureStep NEMA 34 stepper motor):
Controller STP-DRV-6575:
Power Supply:
0.770" Pattern Set Screw Hubs:
Industrial Grade 5RRR1 Block, Flat Mount:
Adafruit 9-DOF Accel/Mag/Gyro+Temp Breakout Board - LSM9DS0:
The control system being used to control the stepper motor has a dual purpose. It not only acts as a complex H-bridge but also manages a high level of accuracy within the system. The schematic depicted above shows the basics involved with controlling the motor. The power supply connects to a basic 120v wall socket and outputs 48 volts DC to the controller. Next the controller accepts a pulse signal/square wave from an Arduino digital pin on an extremely fast delay pulse. The step down allows the controller to turn the stepper motor 1 step. There are 200 steps in one revolution based on the settings I provided to the system. The goal is to model the RPM (revolutions per minute) to a 3rd and 5th order polynomial. Now, the system is operated on a 3rd order polynomial graph between the number of revolutions desired in a certain amount of time and the needed input into the Arduino in microseconds of delay.
Control System: (Excel File: 3rd Order)
Above you can see an image of the Excel file that is used to display the math involved in controlling the motor. All the equations will be displayed in the Arduino code. Due to the complex nature on the equations some need to be simplified before being input in Arduino compared to the Excel file. This is needed because Arduino has a limited amount of time to perform the calculations in real time. Otherwise, an offset will develop changing the desired delay in microseconds it preformed (that is an unwanted action). The 5th order control system is in the testing phase for this project, and It will hopefully become a reality by the end of the semester.
Arduino’s 3rd Order Polynomial System of Equations:
a = (-2.000000*thetaf)/(timef*timef*timef);
b = (3.000000*thetaf)/(timef*timef);
time = micros();
c1 = time/1000000.00;
c2 = a*c1*c1*c1 + b*c1*c1;
c4 = c2-c3;
RPM =(c4/0.001)*60.000000;
c3 = c2;
v = 60.000000/(RPM*0.000400);
Objectives:
- February 16th ------Parts Ordered. (Completed)
- Middle March ------Set up the system and find loads research documentation. (Completed)
- Middle April------Create a working system that can be tested. (Completed)
- Late April______Test the system with different wave forms.
- Early May------Write Final Report and Summarize the progress. Plus, finalize project for future years.
Software:
Current:(Arduino file #1) Main Control File
int maxS = 25;
int sig = 3, direct = 4, enable = 5;
int pot = 0;
float v = 0.000000;
float RPM = 0.000000;
float c1 = 0.000000, c2 = 0.000000, c3 = 0.000000, c4 = 0.000000;
int i = 0, f = 1023;
int inPin1 = 1;
int inPin2 = 2;
int state1 = HIGH;
int state2 = HIGH;
int reading;
int previous = LOW;
unsigned long time;
long debounce = 200;
float thetaf = 100.00000, timef = 20.000000, a = 0.000000, b = 0.000000;
void setup() {
pinMode(sig, OUTPUT);
pinMode(direct, OUTPUT);
pinMode(enable, OUTPUT);
pinMode(inPin1, INPUT);
pinMode(inPin2, INPUT);
// Serial.begin(9600);
}
void loop() {
a = (-2.000000*thetaf)/(timef*timef*timef);
b = (3.000000*thetaf)/(timef*timef);
time = micros();
c1 = time/1000000.00;
c2 = a*c1*c1*c1 + b*c1*c1;
c4 = c2-c3;
RPM =(c4/0.001)*60.000000;
c3 = c2;
v = 60.000000/(RPM*0.000400);
// Serial.print(v);
// Serial.println();
// //Button pressing CODE//
// reading = digitalRead(inPin1);
// if (reading == HIGH & previous == LOW & millis() - time > debounce) {
// if (state1 == HIGH)
// state1 = LOW;
// else
// state1 = HIGH;
//
// time = millis();
// }
// reading = digitalRead(inPin2);
// if (reading == HIGH & previous == LOW & millis() - time > debounce) {
// if (state2 == HIGH)
// state2 = LOW;
// else
// state2 = HIGH;
//
// time = millis();
// }
// //Potentiometer Control Code//
//
// if (state1 == HIGH) {
// digitalWrite(enable, HIGH);
// }
// else {
// digitalWrite(enable, LOW);
// }
// if (state2 == HIGH) {
// digitalWrite(enable, HIGH);
// }
// else {
// digitalWrite(enable, LOW);
// }
// if (v >= 20000) {
// digitalWrite(sig, LOW);
// }
if (v < 20000 & v > maxS) {
digitalWrite(sig, HIGH);
delayMicroseconds(v);
digitalWrite(sig, LOW);
delayMicroseconds(v);
}
//ENDING Statements//
// previous = reading;
}
(Arduino file #1) Linear Logic
if (t == 0) {
setD = setD - 2.5;
if (setT <= 0) {
t = 1;
}
if (setD <= i) {
setT = setT - 1;
setD = 10;
}
}
if (t == 1) {
setD = 10;
setU = setU + 2.5;
if (setT >= f) {
t = 0;
}
if (setU >= 10.00) {
setT = setT + 1;
setU = 0;
}
}
(Arduino file#2) Exponential Creation/Manipulation Logic
if (t == 0) {
setD = setD - 2.5;
if (setT <= 0) {
t = 1;
}
if (setD <= i) {
setT = setT - x;
x = x + 0.05;
setD = 10;
}
}
if (t == 1) {
setD = 10;
setU = setU + 2.5;
if (setT >= f) {
t = 0;
}
if (setU >= 10.00) {
setT = setT + x;
x = x + 0.05;
setU = 0;
}
}
(Arduino file #3)Sine Wave Manipulation/Creation Logic
rad = (theta * 3.14)/ 180;
y = sin(rad);
if(theta < 90){
theta = theta_old - 90;
theta_old = theta_old + var_x;
}
else if (theta == 90 & C < 500){ //hold speed delay
C = C + 1;
}
else if (theta > 90 & theta <= 270){
theta = theta + var_x;
}
else{
Serial.print("break");
}
if (C == 500){
theta = theta + var_x;
C = 0;
}
}
Current: (Matlab file)
% ** Quintic Polynomial Trajectories **
clear all; clc; format compact;
% intial time, displacement, velocity and acceleration
to = 0; qo = 0; qdoto = 0; qddoto = 0;
% final time, displacement, velocity and acceleration
tf = 10; qf = 10; qdotf = 0; qddotf = 0;
%Time interval of the data array
Tstep = 50*abs(to-tf);
% Coefficient matrix for quintic trajectory and its derivative
A = [1, to, to^2, to^3, to^4, to^5; 0, 1, 2*to, 3*to^2, 4*to^3, 5*to^4; 0, 0, 2, 6*to, 12*to^2, 20*to^3; 1, tf, tf^2, tf^3, tf^4, tf^5; 0, 1, 2*tf, 3*tf^2, 4*tf^3, 5*tf^4; 0, 0, 2, 6*tf, 12*tf^2, 20*tf^3];
% Vector of intial and final positions and velocities
b = [qo; qdoto; qddoto; qf; qdotf; qddotf];
% Compute coefficients of trajectory polynomial using
% notion of a = inv(A)*b, but using Gaussian Elimination
a = A\b;
% Evaluate quintic polynomial at times
t = to:(tf-to)/Tstep:tf;
q = a(1) + a(2)*t + a(3)*t.^2 + a(4)*t.^3 + a(5)*t.^4 + a(6)*t.^5;
qdot = a(2) + 2*a(3)*t + 3*a(4)*t.^2 + 4*a(5)*t.^3 + 5*a(6)*t.^4;
qddot = 2*a(3) + 6*a(4)*t + 12*a(5)*t.^2 + 20*a(6)*t.^3;
% Plot trajectories
plot(t,q,'LineWidth',0.5);
legend('q','Location','best');
xlabel('time (sec)');
% s1=serial('com4','BaudRate',9600)
% fopen(s1)
S= mat2str(q);
S= regexprep(S,'([[])[]])','');
S = strrep(S,' ','#');
S
% fwrite(s1,S)