Arizona Chris
Joined: 20 Dec 2014
Posts: 69
Location: Arizona
|
| Parallel input unipolar stepper driver for Robot chip |
 Posted: Fri Nov 06, 2015 6:18 am |
 |
|
This is a solution to the problem of when using standard DC motors for robot locomotion in a differential steering arrangement, the robot cannot drive straight because one motor is always slower than the other and this varies as the payload of the robot changes as well. Measuring accurate distances too is a major headache, mounting shaft encoders on DC motors and monitoring them for similar speeds adds considerable processor overhead and more hardware. We have done this of course in the past, and found for robots that are to be used many months or years pick up dust on the IR sensors for wheel encoders and render them useless after about 2 - 4 months.
The solution is to use stepper motors, and drive them with a precise crystal controlled source. The robot will drive perfectly straight, and know exactly how far it has moved on a level surface.
Project Description
Three parallel control inputs set the direction and speed, and four phases of outputs connect to the motor coil drivers. I have used a ULN device here which is a NPN darlington array in a 16 pin package. I will also be using in the larger robots the TIP120 darlington NPN transistors, with built in back EMF diode. The main stepper chip is one I have many of in my parts drawers, the venerable PIC16F628 device. It is inexpensive and still made today. There are several parameters you can set within the program to work with your existing application. All of our robots in the home environment run with 2 - 4 inch wheels and set for a maximum of 25 - 50rpm. This is the perfect speed for our floor sweeping robots, and plant watering robots to safely move about without getting too out of control. So here we are operating at 25rpm stepper speed with a 3 inch wheel on each side. For your robot to move an exact distance is easy - the time in seconds the motor is on and the RPM of the wheel set that precisely.
The max pulse width (MAXPW) is set here for a reasonable 50mS.
Steps per revolution (SPR) is 48 for our motor here.
RPM (S) is set in code for 25. we were able to go as fast as 100rpm with our small motor as a max.
Function table for three inputs:
inputs: Function:
A2 A1 A0
0 0 0 STOP - NO OUTPUTS (no HOLD)
0 1 0 STOP REVERSE - NO OUTPUTS - RESUMES AT LAST POSITION.
1 0 0 STOP HALF FWD - NO OUTPUTS - RESUMES AT LAST POSITION.
1 1 0 STOP FWD - NO OUTPUTS - RESUMES AT LAST POSITION.
------------
0 0 1 STOP - WITH POSITION HOLD
0 1 1 REVERSE - WITH POSITION HOLD
1 0 1 HALF FWD - WITH POSITION HOLD
1 1 1 FWD - WITH POSITION HOLD
This driver has several advantages for robotics drive systems.\:
1. Drives forward full and half speed for creeping up on targets
2. Stop with hold keeps pulsing the motor at the last position to keep the shaft locked
3. Stop with no hold is for power saving, the outputs are set to zero and no current is drawn when the robot is parked for the night.
4. When starting up again, starts at last stopped position so there is no jumping.
| Code: |
//****************************************************************************
//Chris Schur
//(parallel Stepper 16F628)
//Date: 10/17/15
//****************************************************************************
/*Description of this Program:
This version 1 - First attempt at moving the 887 based program
to this chip - Works great,
Description of this Program:
This is the program that will drive a unipolar stepper with
four phases. The STOP code keeps pulsing the last position before stopping. If
the hold line is low, it drives all four phases low and does not pulse, thus
saving power when the robots drive motors are stopped. It also runs half speed
if selected.*/
//I/O Designations ---------------------------------------------------
// RA0: (AN0) STOP select input
// RA1: (AN1) A1 mode select input
// RA2: (AN2) A2 mode select input
// RA3:
// RA4: (Open Collector output)
// RA5: (MCLR)
// RA6: OSC OUT
// RA7: OSC IN
// RB0: Status LED output
// RB1: LCD output
// RB2: PHASE 4 OUTPUT
// RB3: PHASE 3 OUTPUT
// RB4: PHASE 2 OUTPUT
// RB5: PHASE 1 OUTPUT
// RB6:
// RB7:
/*
==============================================================================
Here is the table of the
actions of this driver:
inputs: Function:
A2 A1 A0
0 0 0 STOP - NO OUTPUTS (no HOLD)
0 1 0 STOP REVERSE - NO OUTPUTS - RESUMES AT LAST POSITION.
1 0 0 STOP HALF FWD - NO OUTPUTS - RESUMES AT LAST POSITION.
1 1 0 STOP FWD - NO OUTPUTS - RESUMES AT LAST POSITION.
------------
0 0 1 STOP - WITH POSITION HOLD
0 1 1 REVERSE - WITH POSITION HOLD
1 0 1 HALF FWD - WITH POSITION HOLD
1 1 1 FWD - WITH POSITION HOLD
------------
PHASE POINTER OUTPUTS DEFINITION:
---------------------------------------
POSITION 1: PHASE1 HIGH, REST LOW
POSITION 2: PHASE2 HIGH, REST LOW
POSITION 3: PHASE3 HIGH, REST LOW
POSITION 4: PHASE4 HIGH, REST LOW
*/
//--------------------------------------------------------------------
//Include Files:
#include <16F628.h> //Normally chip, math, etc. used is here.
//Directives and Defines:
//#device ADC=10 //Set ADC when used to 10 bit = 0 - 1023
#fuses NOPROTECT,HS,NOWDT //xtal is used
#use delay(crystal=10MHz) //xtal speed
#use fast_io(ALL) //must define tris below in main when using this
//for LCD:
#use rs232(baud=9600, xmit=Pin_B1, bits=8, parity=N, stream=SERIALNH)
//NO_ANALOGS is default in device file...
//Constants:
#define MAXPW 50 //Max stepper pulse width
#define MAXRPM 60 //Maximum RPM allowed
#define PHASE1 PIN_B5
#define PHASE2 PIN_B4
#define PHASE3 PIN_B3
#define PHASE4 PIN_B2
#define LED PIN_B0 //status
//***Global Variables:********************************************************
float SPR = 48; //Define the number of steps per revolution here.
int16 P; //total period of all four phases (mS)
int16 PH; //half speed of above
int16 PW; //Pulse width (mS)
int16 PWH; //half speed of above
int16 PR; //Pulse width remainder (only if PW>MAXPW)
int16 PRH; //half speed of above
int8 POINTER; //POINTER to which phase pulse your currently on.
float S = 0; //speed in RPM
float SH; //half speed RPM
int1 DIR; //Direction bit
int1 H; //HALF SPEED BIT
int1 A0; //parallel input 1
int1 A1; //parallel input 2
int1 A2; //parallel input 3
//***Functions/Subroutines, Prototypes:***************************************
//Clears LCD Display:
void LCDCLR() {
fputc(0xFE,SERIALNH); //Command Prefix
fputc(0x51,SERIALNH); //Clear screen
}
//Sets LCD to line 2 start point
void LCDLN2() {
fputc(0xFE,SERIALNH); //Command Prefix
fputc(0x45,SERIALNH); //set cursor command
fputc(0x40,SERIALNH); //Set cursor to next line, pos 40 = start line 2
}
//****************************************************************************
//-- Main Program
//****************************************************************************
void main(void) {
// Set TRIS I/O directions, define analog inputs, comparators:
set_tris_A(0b10111111);
set_tris_B(0b11000000);
//(analog inputs digital by default)
//Initialize variables and Outputs: --------------------------------------
output_low(LED); //status LED off
P = 0;
PW = 0;
PR = 0;
POINTER = 1;
S = 25; //Define your working RPM HERE. Then burn the chip.
delay_ms(1000); //LCD warmup time
LCDCLR();
delay_ms(20);
fprintf(SERIALNH,"STEPPER 1");
delay_ms(20);
LCDLN2();
fprintf(SERIALNH,"RUNNING");
delay_ms(20);
//----------------------------------------------------------------
//----------------------------------------------------------------
//First calculate pulse generation parameters one time for switched operation:
//calculate Period P (time in mS for all four phases)
P = ((240/SPR)/S)*1000; //result is integer 16.
//next calculate pulse width each phase PW: (time in ms for each phase pulse)
PW = P/4;
PR = 0; //otherwise no remainder...
if (PW > MAXPW) { //make sure max PW never exceeds set MAXPW
PW = MAXPW;
PR = (P/4) - MAXPW; //Finally calculate if there is any remainder l
} //for very slow speeds only like < 6 RPM.
//Next we calulate all the half speed pulse data: (Same basic calculations)
SH = S/2;
PH = ((240/SPR)/SH)*1000;
PWH = PH/4;
PRH = 0;
if (PWH > MAXPW) { //make sure max PW never exceeds set MAXPW
PWH = MAXPW;
PRH = (PH/4) - MAXPW; //Finally calculate if there is any remainder l
} //for very slow speeds only like < 6 RPM.
//============================================================================
//MAIN PROGRAM
while (true) { //never leave this loop....
//(Remember driver inverts, so send inverted signal out of uC)
//calculate direction to rotate (DIR):
A0 = (input(PIN_A0));
A1 = (input(PIN_A1)); //read switches
A2 = (input(PIN_A2));
if(A0 == 1)
output_high(LED);
if(A0 == 0)
output_low(LED);
if(A2 == 1 && A1 == 1) { //sw set to FULL speed FWD
DIR = 1;
H = 0; }
if(A2 == 0 && A1 == 1) { //sw setf for FULL speed REV
DIR = 0;
H = 0; }
if(A2 == 1 && A1 == 0) { //sw set to half speed FWD
DIR = 1;
H = 1;
}
//============================================================================
while (DIR == 1 && H == 0 && A0 == 1) { //drive FWD LOOP
switch (POINTER) {
case 1:
output_high(PHASE1); //put out one pulse
delay_ms(PW);
output_low(PHASE1);
delay_ms(PR);
A0 = (input(PIN_A0));
A1 = (input(PIN_A1)); //check inputs
A2 = (input(PIN_A2));
if(A2 == 1 && A1 == 1) { //sw set to FULL speed FWD
DIR = 1;
H = 0; }
if(A2 == 0 && A1 == 1) { //sw setf for FULL speed REV
DIR = 0;
H = 0; }
if(A2 == 1 && A1 == 0) { //sw set to half speed FWD
DIR = 1;
H = 1;
}
if (A1 == 0 && A2 == 0) { //stop and hold if switches are 00
POINTER = 1;
delay_ms(PW);
break; }
POINTER = 2; //otherwise move to next pulse.
break;
case 2:
output_high(PHASE2);
delay_ms(PW);
output_low(PHASE2);
delay_ms(PR);
A0 = (input(PIN_A0));
A1 = (input(PIN_A1));
A2 = (input(PIN_A2));
if(A2 == 1 && A1 == 1) { //sw set to FULL speed FWD
DIR = 1;
H = 0; }
if(A2 == 0 && A1 == 1) { //sw setf for FULL speed REV
DIR = 0;
H = 0; }
if(A2 == 1 && A1 == 0) { //sw set to half speed FWD
DIR = 1;
H = 1;
}
if (A1 == 0 && A2 == 0) {
POINTER = 2;
delay_ms(PW);
break; }
POINTER = 3;
break;
case 3:
output_high(PHASE3);
delay_ms(PW);
output_low(PHASE3);
delay_ms(PR);
A0 = (input(PIN_A0));
A1 = (input(PIN_A1));
A2 = (input(PIN_A2));
if(A2 == 1 && A1 == 1) { //sw set to FULL speed FWD
DIR = 1;
H = 0; }
if(A2 == 0 && A1 == 1) { //sw setf for FULL speed REV
DIR = 0;
H = 0; }
if(A2 == 1 && A1 == 0) { //sw set to half speed FWD
DIR = 1;
H = 1;
}
if (A1 == 0 && A2 == 0) {
POINTER = 3;
delay_ms(PW);
break; }
POINTER = 4;
break;
case 4:
output_high(PHASE4);
delay_ms(PW);
output_low(PHASE4);
delay_ms(PR);
A0 = (input(PIN_A0));
A1 = (input(PIN_A1));
A2 = (input(PIN_A2));
if(A2 == 1 && A1 == 1) { //sw set to FULL speed FWD
DIR = 1;
H = 0; }
if(A2 == 0 && A1 == 1) { //sw setf for FULL speed REV
DIR = 0;
H = 0; }
if(A2 == 1 && A1 == 0) { //sw set to half speed FWD
DIR = 1;
H = 1;
}
if (A1 == 0 && A2 == 0) {
POINTER = 4;
delay_ms(PW);
break; }
POINTER = 1;
break;
} //SWITCH F
} //while forward
//=========================================================================
while (DIR == 0 && H ==0 && A0 == 1) { //drive REV LOOP
switch (POINTER) {
case 1:
output_high(PHASE1);
delay_ms(PW);
output_low(PHASE1);
delay_ms(PR);
A0 = (input(PIN_A0));
A1 = (input(PIN_A1));
A2 = (input(PIN_A2));
if(A2 == 1 && A1 == 1) { //sw set to FULL speed FWD
DIR = 1;
H = 0; }
if(A2 == 0 && A1 == 1) { //sw setf for FULL speed REV
DIR = 0;
H = 0; }
if(A2 == 1 && A1 == 0) { //sw set to half speed FWD
DIR = 1;
H = 1;
}
if (A1 == 0 && A2 == 0) {
POINTER = 1;
delay_ms(PW);
break; }
POINTER = 4;
break;
case 2:
output_high(PHASE2);
delay_ms(PW);
output_low(PHASE2);
delay_ms(PR);
A0 = (input(PIN_A0));
A1 = (input(PIN_A1));
A2 = (input(PIN_A2));
if(A2 == 1 && A1 == 1) { //sw set to FULL speed FWD
DIR = 1;
H = 0; }
if(A2 == 0 && A1 == 1) { //sw setf for FULL speed REV
DIR = 0;
H = 0; }
if(A2 == 1 && A1 == 0) { //sw set to half speed FWD
DIR = 1;
H = 1;
}
if (A1 == 0 && A2 == 0) {
POINTER = 2;
delay_ms(PW);
break; }
POINTER = 1;
break;
case 3:
output_high(PHASE3);
delay_ms(PW);
output_low(PHASE3);
delay_ms(PR);
A0 = (input(PIN_A0));
A1 = (input(PIN_A1));
A2 = (input(PIN_A2));
if(A2 == 1 && A1 == 1) { //sw set to FULL speed FWD
DIR = 1;
H = 0; }
if(A2 == 0 && A1 == 1) { //sw setf for FULL speed REV
DIR = 0;
H = 0; }
if(A2 == 1 && A1 == 0) { //sw set to half speed FWD
DIR = 1;
H = 1;
}
if (A1 == 0 && A2 == 0) {
POINTER = 3;
delay_ms(PW);
break; }
POINTER = 2;
break;
case 4:
output_high(PHASE4);
delay_ms(PW);
output_low(PHASE4);
delay_ms(PR);
A0 = (input(PIN_A0));
A1 = (input(PIN_A1));
A2 = (input(PIN_A2));
if(A2 == 1 && A1 == 1) { //sw set to FULL speed FWD
DIR = 1;
H = 0; }
if(A2 == 0 && A1 == 1) { //sw setf for FULL speed REV
DIR = 0;
H = 0; }
if(A2 == 1 && A1 == 0) { //sw set to half speed FWD
DIR = 1;
H = 1;
}
if (A1 == 0 && A2 == 0) {
POINTER = 4;
delay_ms(PW);
break; }
POINTER = 3;
break;
} //SWITCH R
} //while reverse
//========================================================================
//============================================================================
while (DIR == 1 && H == 1 && A0 == 1) { //drive HALF FWD LOOP
switch (POINTER) {
case 1:
output_high(PHASE1); //put out one pulse
delay_ms(PWH);
output_low(PHASE1);
delay_ms(PRH);
A0 = (input(PIN_A0));
A1 = (input(PIN_A1)); //check inputs
A2 = (input(PIN_A2));
if(A2 == 1 && A1 == 1) { //sw set to FULL speed FWD
DIR = 1;
H = 0; }
if(A2 == 0 && A1 == 1) { //sw setf for FULL speed REV
DIR = 0;
H = 0; }
if(A2 == 1 && A1 == 0) { //sw set to half speed FWD
DIR = 1;
H = 1;
}
if (A1 == 0 && A2 == 0) { //stop and hold if switches are 00
POINTER = 1;
delay_ms(PWH);
break; }
POINTER = 2; //otherwise move to next pulse.
break;
case 2:
output_high(PHASE2);
delay_ms(PWH);
output_low(PHASE2);
delay_ms(PRH);
A0 = (input(PIN_A0));
A1 = (input(PIN_A1));
A2 = (input(PIN_A2));
if(A2 == 1 && A1 == 1) { //sw set to FULL speed FWD
DIR = 1;
H = 0; }
if(A2 == 0 && A1 == 1) { //sw setf for FULL speed REV
DIR = 0;
H = 0; }
if(A2 == 1 && A1 == 0) { //sw set to half speed FWD
DIR = 1;
H = 1;
}
if (A1 == 0 && A2 == 0) {
POINTER = 2;
delay_ms(PWH);
break; }
POINTER = 3;
break;
case 3:
output_high(PHASE3);
delay_ms(PWH);
output_low(PHASE3);
delay_ms(PRH);
A0 = (input(PIN_A0));
A1 = (input(PIN_A1));
A2 = (input(PIN_A2));
if(A2 == 1 && A1 == 1) { //sw set to FULL speed FWD
DIR = 1;
H = 0; }
if(A2 == 0 && A1 == 1) { //sw setf for FULL speed REV
DIR = 0;
H = 0; }
if(A2 == 1 && A1 == 0) { //sw set to half speed FWD
DIR = 1;
H = 1;
}
if (A1 == 0 && A2 == 0) {
POINTER = 3;
delay_ms(PWH);
break; }
POINTER = 4;
break;
case 4:
output_high(PHASE4);
delay_ms(PWH);
output_low(PHASE4);
delay_ms(PRH);
A0 = (input(PIN_A0));
A1 = (input(PIN_A1));
A2 = (input(PIN_A2));
if(A2 == 1 && A1 == 1) { //sw set to FULL speed FWD
DIR = 1;
H = 0; }
if(A2 == 0 && A1 == 1) { //sw setf for FULL speed REV
DIR = 0;
H = 0; }
if(A2 == 1 && A1 == 0) { //sw set to half speed FWD
DIR = 1;
H = 1;
}
if (A1 == 0 && A2 == 0) {
POINTER = 4;
delay_ms(PWH);
break; }
POINTER = 1;
break;
} //SWITCH HF
} //while H forward
//=========================================================================
//STOP ALL LOOP - for setting all outputs low for power saving when stopped
while (A0 == 0) {
output_low(PHASE1);
output_low(PHASE2);
output_low(PHASE3);
output_low(PHASE4);
break;
} //stop loop
} //while true
} //main
//********* Functions which have prototypes at top of program ****************
|
|
|
FAQ 
Search
Register
Profile 
Log in to check your private messages 
Log in
