Post on 26-Sep-2020
Soft Motion Motor Control & Trajectory Planning
By: Nicholas Curry May 5th, Spring 2017
Instructor: Dr. Cris KoutsougerasAdvisor: Dr. Mohammad Saadeh
Abstract:
The goal of this project is to operate a motor with precision while minimizing the effect
of jerk. The effects of jerk can cause damage and vibration not needed in the system. Jerk is
minimized by creating a fifth order polynomial trajectory of the motor’s shaft displacement.
Furthermore, soft motion is a type of trajectory that has a smooth transition from rest-to-rest. The
soft motion trajectory was laid out in Matlab and converted to python for control of the motor.
However, the difference between a soft motion and fifth order polynomial trajectory show both
audibly and visually in the systems performance. The intent is to develop a software that can
produce a trajectory broken up into seven equally divided segments, and ensure each segment
transitions without discontinuity. By establishing the seven different segments we can minimize
vibration in the system and gives the engineer editability of each segment of the trajectory.
Moreover, all the program needs to run are two inputs. They are total run time and total number
of revolutions. The software also gives multiple trajectories and real time adjustment tools to the
user. Running the program will calculate the trajectory and send a pin high and low. Thus,
producing the desired frequency needed for the stepper motor to follow the set trajectory. Final,
using this software will make hardware accurately follow the users preplanned 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 robotic of all types. These displacement systems are
often controlled from rest-to-rest and can be optimized using soft motion. However, the
important part is not always the starting and ending points but rather the velocity, acceleration
and jerk acting on the system. For instance, the shortest path may be the most efficient, but can
cause harm to the transfer system or transferee if large amounts of vibration or change in speed
are introduced. Many systems need motors to move mechanisms while applying soft motion
trajectories. These trajectories can help improve the performance of a system, thus causing less
harm to the objects and tools.
This project was tested using a stepper motors control system from Automation Direct®.
The processor and programming languages include an Arduino board, Raspberry Pi, and
MATLAB somewhere along the process. First, an Arduino Uno was used to implement the
trajectory. It was filter out as a poor processor at this level of control. Because of this discovery,
the Raspberry Pi was used to replace the Arduino Uno. The Raspberry Pi has enough memory to
store the required data points, and handling all the calculations needed to compute the time delay
array.
Parts:
Motor STP-MTRH-34127: (SureStep NEMA 34 stepper motor):
Controller STP-DRV-6575:
Power Supply:
Raspberry Pi 2 Setup: (Any USB Mouse and Keyboard with a HDMI Ported Display System)
System Layout & Design:
The control system used to control the stepper motor has a dual purpose. It does not only
act as an H-bridge, but also maintains a high level of accuracy within the system. The schematic
depicted below shows the basics involved with controlling the motor. The power supply connects
to a 120v wall socket and outputs 48 DC Volts and 10 Amps to the controller. The Raspberry Pi
python program turns a digital pin High and Low. Using a delay timer to control the frequency of
the pulse to the controller. The motor controller uses that pulse to send voltage to the motor. The
motor shaft rotated 0.05 revolutions or one step each time the digital pin is turned from high to
low. The step down allows the controller to turn the stepper motor 1 step. There are 200 steps in
one revolution based on the selected control configuration.
To configure a trajectory, the system uses a four-button setup that choses the number of
revolutions, total travel time, direction of either clock-wise or counter clock-wise and trajectory
type of either soft motion or fifth order polynomial. An additional button enables the run mode
through applying the trajectory configuration chosen earlier. This includes reading the recorded
data, calculating the necessary delay array values and running the correct program.
Picture of the setup with motor mount to a conveyer belt for testing purposes:
Software: (Python file) Stepper Motor Control
Button_Press_All _Python FileNotes: This program controls the entire system. It rotates the motor’s shaft at an input number of revolutions and at an input time from the user. The program sends the user inputs from the buttons to the correct trajectory. The selected trajectory is calculated and the system runs the motor along that path and to the desired position. After the trajectory is complete the system immediately returns to the starting position.
import numpy as npimport timeimport RPi.GPIO as GPIOimport datetimeimport os#Setup Pins for writing
GPIO.setmode(GPIO.BCM)GPIO.setwarnings(False)GPIO.setup(18,GPIO.IN,pull_up_down=GPIO.PUD_UP) #phycial 11, BCM 18( button)
GPIO.setup(23,GPIO.IN,pull_up_down=GPIO.PUD_UP) #phycial 11, BCM 23( button)GPIO.setup(24,GPIO.IN,pull_up_down=GPIO.PUD_UP) #phycial 11, BCM 24( button)GPIO.setup(25,GPIO.IN,pull_up_down=GPIO.PUD_UP) #phycial 11, BCM 25( button)GPIO.setup(12,GPIO.IN,pull_up_down=GPIO.PUD_UP) #phycial 11, BCM 12( button)GPIO.setup(27,GPIO.OUT) #This send direction signal to Controller, phycial 11, BCM 17
trajectory1 = 0trajectory2 = 0DirectionC = 0TrajectoryC = 0timeC = 0.5revolutionC = 0.5os.system(‘clear’)
while True: start = GPIO.input(18) direction = GPIO.input(23) trajectory = GPIO.input(24) timef = GPIO.input(25) revolution = GPIO.input(12) time.sleep(0.1) if direction == False: DirectionC = DirectionC + 1 print'Direction' print(DirectionC % 2) if trajectory == False: TrajectoryC = TrajectoryC + 1 print'Trajectory Type' print(TrajectoryC % 2) if timef == False: timeC = timeC + 0.5 print'Time Final' print(timeC) if revolution == False: revolutionC = revolutionC + 0.5 print'Revolutions' print(revolutionC) if timeC == 10.5: timeC = 0.5 print'Time Final' print(timeC) if revolutionC == 7.5: revolutionC = 0.5 print'Revolutions' print(revolutionC) if start == False:
os.system(‘clear’) print'Trajectory Type' print(TrajectoryC % 2) print'Time Final' print(timeC) print'Revolutions' print(revolutionC) print'Time Final' print(timeC) print'Direction'
print(DirectionC % 2) if DirectionC % 2 == 0: GPIO.output(17,False) #Go CC Direction --Looking From Top print "Rotating Clock-Wise" if DirectionC % 2 == 1: GPIO.output(17,True) #Go CCW Direction print "Rotating Counter-Clock-Wise" if TrajectoryC % 2 == 0: print 'Please wait while FO_trajectory is calculated...' trajectory1 = fo.calc_fifth_order(timeC,revolutionC) print 'Ready to Rock and Roll' fo.run_motor(trajectory1) DirectionC = DirectionC + 1 if TrajectoryC % 2 == 1: print 'Please wait while SM_trajectory is calculated...' trajectory2 = sm.calc_soft_motion(timeC,revolutionC) print 'Ready to Rock and Roll' sm.run_motor(trajectory2) DirectionC = DirectionC + 1 if DirectionC % 2 == 0: GPIO.output(17,False) #Go CC Direction --Looking From Top print "Rotating Clock-Wise" if DirectionC % 2 == 1: GPIO.output(17,True) #Go CCW Direction print "Rotating Counter-Clock-Wise" if TrajectoryC % 2 == 0: print 'Returning to start position' fo.run_motor(trajectory1) if TrajectoryC % 2 == 1: print 'Returning to start position' sm.run_motor(trajectory2)
Soft_motion _Python File
import numpy as npimport timeimport RPi.GPIO as GPIOimport datetime
#Setup Pins for writing
GPIO.setmode(GPIO.BCM)GPIO.setwarnings(False)GPIO.setup(26,GPIO.OUT) #Using GPIO BCM pin 26, which is physical 25
def calc_soft_motion(timeC,revolutionC):
Thi = 0.00
t = timeC
Thmax = revolutionC
Thf = Thi + Thmax
m1 =0.0233235
m2 = m1 *pow(t,3)
Jmax = Thmax/m2
#Soft Motion Trajectory
n = Thmax/0.0001
T = np.linspace(0.0,t,8) #divide time into 7 segments
A0 = 0.0 #initial acceleration
V0 = 0.0 #intial velocity
Th0 = Thi #inital displacement
J0 = Jmax
#1st segment
i = 0 #first
T1 = np.linspace(T[i],T[i+1],n)
J1 = J0*np.ones((1,n))
A1 = A0 + J0*T1
V1 = V0 + A0*T1+ 0.5*J0*T1**2
Th1 = (Th0 + V0*T1)+(0.5*A0*T1**2)+((1/6.0)*J0*T1**3)
Amax1 = A1[len(A1)-1]
Vmax1 = V1[len(V1)-1]
Thmax1 = Th1[len(Th1)-1]
#2nd segment
i = 1 #second
T2 = np.linspace(T[i],T[i+1],n)
J2 = np.zeros((1,n))
A2 = Amax1*np.ones((1,n))
V2 = Vmax1 + Amax1*(T2-T[1])
Th2 = Thmax1+Vmax1*(T2-T[1]) + 0.5*Amax1*(T2-T[1])**2
Amax2 = A2[len(A2)-1]
Vmax2 = V2[len(V2)-1]
Thmax2 = Th2[len(Th2)-1]
#3rd segment
i = 2 #third
T3 = np.linspace(T[i],T[i+1],n)
J3 = -J0*np.ones((1,n))
A3 = Amax2 - J0*(T3-T[2])
V3 = Vmax2 +Amax2*(T3-T[2]) - 0.5*J0*(T3-T[2])**2
Th3 = Thmax2 + Vmax2 *(T3-T[2])+0.5*Amax2*(T3-T[2])**2-(1/6.0)*J0*(T3-T[2])**3
Amax3 = A3[len(A3)-1]
Vmax3 = V3[len(V3)-1]
Thmax3 = Th3[len(Th3)-1]
#4th segment
i = 3 #four
T4 = np.linspace(T[i],T[i+1],n)
J4 = np.zeros((1,n))
A4 = np.zeros((1,n))
V4 = Vmax3*np.ones((1,n))
Th4 = Thmax3 + Vmax3*(T4-T[3])
Amax4 = A4[len(A4)-1]
Vmax4 = V4[len(V4)-1]
Thmax4 = Th4[len(Th4)-1]
#5th segment
i = 4 #fifth
T5= np.linspace(T[i],T[i+1],n)
J5 = -J0*np.ones((1,n))
A5 = Amax4 - J0*(T5-T[4])
V5 = Vmax4 +Amax4*(T5-T[4]) - 0.5*J0*(T5-T[4])**2
Th5 = Thmax4 + Vmax4 *(T5-T[4])+0.5*Amax4*(T5-T[4])**2-(1/6.0)*J0*(T5-T[4])**3
Amax5 = A5[len(A5)-1]
Vmax5 = V5[len(V5)-1]
Thmax5 = Th5[len(Th5)-1]
#6th segment
i = 5 #sixth
T6 = np.linspace(T[i],T[i+1],n)
J6 = np.zeros((1,n))
A6 = Amax5*np.ones((1,n))
V6 = Vmax5 + Amax5*(T6-T[5])
Th6 = Thmax5+Vmax5*(T6-T[5]) + 0.5*Amax5*(T6-T[5])**2
Amax6 = A6[len(A6)-1]
Vmax6 = V6[len(V6)-1]
Thmax6 = Th6[len(Th6)-1]
#7th segment
i = 6 #seventh
T7 = np.linspace(T[i],T[i+1],n)
J7 = J0*np.ones((1,n))
A7 = Amax6+J0*(T7-T[6])
V7 = Vmax6 +Amax6+(T7-T[6])+0.5*J0*(T7-T[6])**2
Th7 = Thmax6+Vmax6*(T7-T[6])+0.5*Amax6*(T7-T[6])**2+(1/6.0)*J0*(T7-T[6])**3
Amax7 = A7[len(A7)-1]
Vmax7 = V7[len(V7)-1]
Thmax7 = Th7[len(Th7)-1]
Time = [T1,T2,T3,T4,T5,T6,T7]
Th = [Th1,Th2,Th3,Th4,Th5,Th6,Th7]
#Convert to Numpy Objs
Th = np.asarray(Th).flatten()
Time = np.asarray(Time).flatten()
dt_chart =[]
dth_chart = []
Po = 0.0
i= 0
for index, item in enumerate(Th):
Thi = Po+Th[i]
if index < len(Th)-1:
dth = abs(Thi - Th[index+1]) else:
dth = abs(Thi - Th[index])
dth_chart.append(dth)
if dth >= 0.005:
dt = Time[index] - Time[i]
Po = dth-0.005
dt_chart.append(dt)
i = index #print len(dt_chart)
return dt_chart
def run_motor(dt_chart):
for item in dt_chart:
time.sleep(item)
GPIO.output(26,1)
time.sleep(item)
GPIO.output(26,0)
5th_Order_Polynomial _Python File
import RPi.GPIO as GPIO
import time
import datetime
#Setup Pins for writing
GPIO.setmode(GPIO.BCM)
GPIO.setwarnings(False)
GPIO.setup(26,GPIO.OUT) #Using GPIO BCM pin 26, which is physical 25
def calc_fifth_order(timeC,revolutionC): time_final = timeC #final time in seconds theta_final = revolutionC #Number of revolutions a = (6.0 * theta_final)/pow(time_final, 5) b = (-15.0 * theta_final)/pow(time_final, 4) c = (10.0 * theta_final)/pow(time_final, 3)
#Time Values setup T = 0.0 T_step = 0.0001 T_old = 0.0 th = 0.0
th_old = 0.0 #Time delay collection dt_chart = [] dt = 0.0
for _ in range(int(time_final/T_step)):
T = T + T_step
th = (a*pow(T,5))+(b*pow(T,4))+(c*pow(T,3))
dth = abs(th_old -th)
if(dth >= 0.005):
dt = T -T_old
T_old = T
th_old = th - (dth -0.005)
dt = dt/2
dt_chart.append(dt) #print('Running Motor....')
return dt_chart
def run_motor(dt_chart):
for item in dt_chart:
time.sleep(item)
GPIO.output(26,1)
time.sleep(item)
GPIO.output(26,0)
Current: (Matlab files Used to Create the python files) NOTE: This file’s existence is redundant but because it was created first and became obsolete. However, was still put to good use. It can be used and was used against the other python program’s output. Allowing us to check for programming errors or inconstancies.5th_Order_control_system:
clc; clear; format compact;Tf = 4; %Exicution timeThf = 12; %Distance Traveled in Revolutionsa = 6 * Thf / Tf^5; %Part A
b = -15 * Thf / Tf^4; %Part Bc = 10 * Thf / Tf^3; %Part CT = 0; %This is set and the World TimeT_step = 0.0001; %Size of Time stepi = 0; %Used to run For loopth = 0; %Displacement in Revolutionsth_old = 0; %Last Calculated Displacement in Revolutionstimechart = [0]; %Output data for T%------------------------------------------------------------------------dtchart = [0]; %Collects all properly filtered "dt" values Data for time between each step% THIS DATA WILL BE USED IN CONTROLLER% AND%CONTROL THE DELAY BETWEEN HIGH AND LOW VALUES%------------------------------------------------------------------------theta_out = [0]; %Output data for displacement of ThetaDATASHEET = []; %Only used for Data Anylysisdt = 0; %Change in Time/StepT_old = 0; %Place holder of past time for i = 0:T_step:Tf %For LOOP (Tf/T_step =#steps checked) T = T + T_step; %Virtual Timer th = a*T^5 + b*T^4 + c*T^3; %5th Order Polynomial Equation for Theta@T dth = abs(th_old - th); %Used to filter unwanted control data if dth >= 0.005 dt = T - T_old; %[IMPORTANT]Finds change in time between each step T_old = T; %Saves old time th_old = th - (dth - 0.005); %Sets new Range for dth theta_out = [theta_out,th]; %Complies Array of Th timechart = [timechart,T]; %Complies Array of T dt = round(dt * 1000000); dtchart = [dtchart,dt]; %Complies Array of dt endendtheta_out = theta_out(:,2:end); %Remove unwanted end valuestimechart = timechart(:,2:end); %Remove unwanted end valuesdtchart = dtchart(:,2:end); %Remove unwanted end valuesDATASHEET = [dtchart;timechart;theta_out]; %All Outputs In One Array
Current: (Matlab files) Soft Motion Control System:
Input Data: (Time Final OR Total Time); (Theta Final OR Total Number of Revolutions)Output Data: (Array of Δt/step OR 1.8º OR 0.05 revolutions); (Output chart)Output Chart When: (Tf = 4); (Thf = 4)
clc;clear;close allformat compactformat long% trajectory specs Thi = 0;t = 4; % total timeThmax = 4; % final angle in radians (relative angle (Thf - Thi)Thf = Thi + Thmax;m1 = 0.0233235;m2 = m1 * t^3;Jmax = Thmax / m2;% Soft Motion Trajectoryn = Thmax/0.0001;T = linspace(0,t,8); % divide time into 7 equal segments A0 = 0; % initial accelerationV0 = 0; % intial velocityTh0 = Thi; % initial displacementJ0 = Jmax; % 1st segmenti = 1; % firstT1 = linspace(T(i),T(i+1),n);J1 = J0*ones(1,n);A1 = A0 + J0*T1;V1 = V0 + A0*T1 + 1/2*J0*T1.^2;
Th1 = Th0 + V0*T1 + 1/2*A0*T1.^2 + 1/6*J0*T1.^3;Amax1 = A1(end);Vmax1 = V1(end);Thmax1 = Th1(end); % 2nd segmenti = 2;T2 = linspace(T(i),T(i+1),n);J2 = zeros(1,n);A2 = Amax1*ones(1,n);V2 = Vmax1 + Amax1*(T2-T(2));Th2 = Thmax1+ Vmax1*(T2-T(2)) + 1/2*Amax1*(T2-T(2)).^2;Amax2 = A2(end);Vmax2 = V2(end);Thmax2 = Th2(end); % 3rd segmenti = 3;T3 = linspace(T(i),T(i+1),n);J3 = -J0*ones(1,n);A3 = Amax2 - J0*(T3-T(3));V3 = Vmax2 + Amax2*(T3-T(3)) - 1/2*J0*(T3-T(3)).^2;Th3 = Thmax2 + Vmax2*(T3-T(3)) + 1/2*Amax2*(T3-T(3)).^2 - 1/6*J0*(T3-T(3)).^3;Amax3 = A3(end);Vmax3 = V3(end);Thmax3 = Th3(end); % 4th segmenti = 4;T4 = linspace(T(i),T(i+1),n);J4 = zeros(1,n);A4 = zeros(1,n);V4 = Vmax3*ones(1,n) ;Th4 = Thmax3 + Vmax3*(T4-T(4));Amax4 = A4(end);Vmax4 = V4(end);Thmax4 = Th4(end); % 5th segmenti = 5; % T5 = linspace(T(i),T(i+1),n);J5 = -J0*ones(1,n);A5 = Amax4 - J0*(T5-T(5));V5 = Vmax4 + Amax4*(T5-T(5)) - 1/2*J0*(T5-T(5)).^2;Th5 = Thmax4 + Vmax4*(T5-T(5)) + 1/2*Amax4*(T5-T(5)).^2 - 1/6*J0*(T5-T(5)).^3;Amax5 = A5(end);Vmax5 = V5(end);Thmax5 = Th5(end); % 6th segmenti = 6;T6 = linspace(T(i),T(i+1),n);J6 = zeros(1,n);A6 = Amax5*ones(1,n);V6 = Vmax5 + Amax5*(T6-T(6));
Th6 = Thmax5+ Vmax5*(T6-T(6)) + 1/2*Amax5*(T6-T(6)).^2;Amax6 = A6(end);Vmax6 = V6(end);Thmax6 = Th6(end); % 7th segmenti = 7; %seventh T7 = linspace(T(i),T(i+1),n);J7 = J0*ones(1,n);A7 = Amax6 + J0*(T7-T(7));V7 = Vmax6 + Amax6*(T7-T(7)) + 1/2*J0*(T7-T(7)).^2;Th7 = Thmax6 + Vmax6*(T7-T(7)) + 1/2*Amax6*(T7-T(7)).^2 + 1/6*J0*(T7-T(7)).^3;Amax7 = A7(end);Vmax7 = V7(end);Thmax7 = Th7(end); Time = [T1 T2 T3 T4 T5 T6 T7];J = [J1 J2 J3 J4 J5 J6 J7];A = [A1 A2 A3 A4 A5 A6 A7];V = [V1 V2 V3 V4 V5 V6 V7];Th = [Th1 Th2 Th3 Th4 Th5 Th6 Th7];dtchart =[];dthchart =[]; i = 1;n = (n*7) - 1;Po = 0;for c = 1:n Thi = Po + Th(i); dth = abs(Thi - Th(c + 1)); if dth >= 0.005 dt = Time(c) - Time (i); Po = dth - 0.005; dtchart = [dtchart(), ]; i = c; endend plot(Time,J,Time,A,Time,V,Time,Th,'LineWidth',2)grid onlegend('Jerk','Acceleration','Velocity','Displacement','Location','NorthWest')
Arduino Checking Position System:Using the Ultrasonic Ping Sensor and an Arduino Uno to analyze the trajectory.Output: Positon data of the track for analytical use
const int pingPin = 7;void setup() { Serial.begin(9600);}
void loop() {
float duration, inches, cm;
pinMode(pingPin, OUTPUT); digitalWrite(pingPin, LOW); delayMicroseconds(2); digitalWrite(pingPin, HIGH); delayMicroseconds(5); digitalWrite(pingPin, LOW);
pinMode(pingPin, INPUT); duration = pulseIn(pingPin, HIGH);
cm = microsecondsToCentimeters(duration);
Serial.print(cm); Serial.println(); delay(100);}
float microsecondsToCentimeters(float microseconds) { return microseconds / 29 / 2;}
Arduino Checking Acceleration of the System:Using the LSM9DS0 and an Arduino Uno to analyze the Acceleration.Output: Acceleration data of the track for analytical use
#include <SPI.h>#include <Wire.h>#include <Adafruit_Sensor.h>#include <Adafruit_LSM9DS0.h>float old_accel = 0.00;float max_pos_accel = 0.00;float max_neg_accel = 0.00;float new_pos_accel = 0.00;float new_neg_accel = 0.00;/* Assign a unique base ID for this sensor */Adafruit_LSM9DS0 lsm = Adafruit_LSM9DS0(1000); // Use I2C, ID #1000
void displaySensorDetails(){ sensor_t accel, mag, gyro, temp;
lsm.getSensor(&accel, &mag, &gyro, &temp);}
void configureSensor(void){ //Set the accelerometer range @ ( 2G,4G,6G,8G,or16G ) lsm.setupAccel(lsm.LSM9DS0_ACCELRANGE_2G);}
void setup(){#ifndef ESP8266 while (!Serial); // will pause Zero, Leonardo, etc until serial console opens
#endif Serial.begin(9600);
/* Initialise the sensor */ if (!lsm.begin()) { /* There was a problem detecting the LSM9DS0 ... check your connections */ Serial.print(F("Ooops, no LSM9DS0 detected ... Check your wiring or I2C ADDR!")); while (1); } Serial.println(F("Found LSM9DS0 9DOF"));
/* Display some basic information on this sensor */ displaySensorDetails();
/* Setup the sensor gain and integration time */ configureSensor();}
void loop(){ if (new_pos_accel > max_pos_accel){ max_pos_accel = new_pos_accel; } if (new_neg_accel < max_neg_accel){ max_neg_accel = new_neg_accel; } /* Get a new sensor event */ sensors_event_t accel, mag, gyro, temp;
lsm.getEvent(&accel, &mag, &gyro, &temp);
// print out accelleration data accel.acceleration.x = accel.acceleration.x + 1; if (accel.acceleration.x >= 0.08) { accel.acceleration.x = accel.acceleration.x - 0.07; Serial.print("Accel X: "); Serial.print(accel.acceleration.x); Serial.println("m/s^2"); Serial.println("**********************\n"); Serial.print("Accel Max_pos X: "); Serial.print(max_pos_accel); Serial.println(); Serial.print("Accel Max_neg X: "); Serial.print(max_neg_accel); Serial.println(); Serial.println("**********************\n"); accel.acceleration.x = new_pos_accel; } if (accel.acceleration.x <= -0.08) { accel.acceleration.x = accel.acceleration.x + 0.07; Serial.print("Accel X: "); Serial.print(accel.acceleration.x); Serial.println("m/s^2"); Serial.println("**********************\n"); Serial.print("Accel Max_pos X: "); Serial.print(max_pos_accel); Serial.println(); Serial.print("Accel Max_neg X: "); Serial.print(max_neg_accel); Serial.println(); Serial.println("**********************\n"); accel.acceleration.x = new_neg_accel; }}