€¦ · Web viewThe processor and programming languages include an Arduino board, Raspberry Pi,...

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


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




#3rd segment

i = 2 #third


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




#4th segment

i = 3 #four



A4 = np.zeros((1,n))

V4 = Vmax3*np.ones((1,n))

Th4 = Thmax3 + Vmax3*(T4-T[3])




#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




#6th segment

i = 5 #sixth



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




#7th segment

i = 6 #seventh


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




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; }}

€¦ · Web viewThe processor and programming languages include an Arduino board, Raspberry Pi,...

Documents

Transcript of €¦ · Web viewThe processor and programming languages include an Arduino board, Raspberry Pi,...