Joseph Bradshaw
Serial interface for controlling robotic arm.
This program uses a LPC1768 processor for controlling a robotic arm. The basis for the program is the Axis () class which uses a PID controller to actuate a DC motor with quadrature encoder feedback.
Youtube video of robotic arm using the 6-axis controller performing an internally programmed gate, then performing the home function.
https://developer.mbed.org/users/jebradshaw/code/Axis/
The Axis Class has 3 dependencies (MotCon, LS7366LIB, and PID). The class encapsulates the required functionality of controlling a DC motor with encoder feedback through pin assignments, an SPI bus, and a pointer for the limit switch source.
The LS7366 encoder interface IC off-loads the critical time and counting requirements from the processor using an SPI bus interface for the class. The Axis class then uses a state machine to perform trapezoidal movement profiles with a Ticker class. Parameters can be adjusted through the serial interface using a FT232RL USB to serial interface IC for computer communication.
The MotCon class is a basic class that defines a PWM output pin and a single direction signal intended to control an H-Bridge motor driver IC. I used an MC33926 motor driver for each motor which are rated at 5.0-28V and 5.0 amp peak, with an RDSon max resistance of 225 milli-ohms. This part also has 3.0V to 5V TTL/CMOS inputs logic levels and various protection circuitry on board. I also liked this particular motor driver chip because you can use a PWM frequency of up to 20KHz, getting the frequency out of the audio range.
Above is the prototype for the controller. Originally, a PCF8574 I/O expander was used to read the limit switches by the I2C bus. This has now been re-written to use 6 external interrupts directly for the limit/homing switches. Six motor driver breakout boards using the MC33926 motor driver chip were used to drive the motors.
I use the mbed online compiler to generate the .bin file, use bin2hex to convert it and upload the hex file using Flash Magic to the processor with the serial bootloader. I prefer to use the FT232RL usb to serial converter IC for PC comms due to the high level of reliability and USB driver support (typically already built in Windows 7+). I've started putting this on a PCB and hope to finish by the end of the month (Dec 2015).
Well 3 months later, I've completed the first PCB prototype. A few minor errors but it's working!!
Express PCB Artwork
Inner Power Layer Breakup for motor current
First Prototype
Latest documentation (schematic and parts layout) can be found here Download and open with Adobe /media/uploads/jebradshaw/axiscontroller_schematics_v2.0.pdf
Latest PCB File (Express PCB) /media/uploads/jebradshaw/lpc1768_axiscontroller_20161216.pcb
Parts Layout
Python script for converting mbed .bin output to intel hex format (no bin2hex 64K limit) https://pypi.python.org/pypi/IntelHex
Example batch script for speeding up conversion process for FlashMagic (http://www.flashmagictool.com/) programming of board /media/uploads/jebradshaw/axisconvert.bat
https://os.mbed.com/users/jebradshaw/code/axis_ScorbotController_20180706/
Latest firmware: 20190823 - /media/uploads/jebradshaw/axis_scorbotcontroller_20190823_velocity_test.lpc1768.bin
Diff: lpc_axis.cpp
- Revision:
- 1:0d7a1f813571
- Parent:
- 0:02d2a7571614
- Child:
- 2:e5d56ee55af6
--- a/lpc_axis.cpp Fri Sep 25 19:28:01 2015 +0000
+++ b/lpc_axis.cpp Wed Sep 30 16:21:14 2015 +0000
@@ -9,12 +9,13 @@
#include "mbed.h"
#include "Axis.h"
#include "stdlib.h"
-#include "PCF8574.h" //library for the
+#include "PCF8574.h" //library for the I/O expander (limit switches)
#include <string>
#define PI 3.14159
#define PCF8574_ADDR 0 // I2c PCF8574 address is 0x00
+#define SP_TOL 100 // SET POINT TOLERANCE is +/- tolerance for set point command
DigitalOut led1(P1_18); //blue
DigitalOut led2(P1_20); //
@@ -26,13 +27,13 @@
int limit0, limit1, limit2, limit3, limit4, limit5; //global limit switch values
Timer t;
-//instantiate axis object
-Axis axis1(spi, P1_24, P2_5, P1_0, &limit0); //spi bus, LS7366_cs, pwm, dir, limitSwitch
-Axis axis2(spi, P1_25, P2_4, P1_1, &limit1);
-Axis axis3(spi, P1_26, P2_3, P1_4, &limit2);
-Axis axis4(spi, P1_27, P2_2, P1_8, &limit3);
-Axis axis5(spi, P1_28, P2_1, P1_9, &limit4);
-Axis axis6(spi, P1_29, P2_0, P1_10, &limit5);
+//instantiate axis object NAME(spi bus, LS7366_cs, pwm, dir, analog, limitSwitchAddress, TotalEncoderCounts/axis)
+Axis axis1(spi, P1_24, P2_5, P1_0, P0_23, &limit0, 25000.0); //base
+Axis axis2(spi, P1_25, P2_4, P1_1, P0_24, &limit1, 17500); //shoulder
+Axis axis3(spi, P1_26, P2_3, P1_4, P0_25, &limit2, 20500); //elbow
+Axis axis4(spi, P1_27, P2_2, P1_8, P0_26, &limit3, 5000); //pitch/roll
+Axis axis5(spi, P1_28, P2_1, P1_9, P1_30, &limit4, 5000); //pitch/roll
+Axis axis6(spi, P1_29, P2_0, P1_10, P1_31, &limit5, 5400); //grip
PCF8574 pcf(P0_10,P0_11,PCF8574_ADDR,false); // Declare PCF8574 i2c with sda and scl (p28,p27) (10K pullups!)
//uint8_t data;
@@ -85,39 +86,50 @@
}
void home_test(void){
- axis1.pid->setInputLimits(-20000.0, 20000.0);
- while(*axis1.ptr_limit == 1){
- axis1.set_point += 100;
- axis1.pid->setSetPoint(axis1.set_point);
+ axis2.pid->setInputLimits(-20000.0, 20000.0);
+ while(*axis2.ptr_limit == 1){
+ axis2.set_point += 100;
+ axis2.pid->setSetPoint(axis2.set_point);
- axis2.set_point -= 100;
- axis2.pid->setSetPoint(axis2.set_point);
+ axis3.set_point -= 100;
+ axis3.pid->setSetPoint(axis3.set_point);
wait(.05);
if(axis1.debug)
printf("T=%.2f SP=%.3f co=%.3f pos=%.3f vel=%.3f acc=%.3f limit=%d\r\n", t.read(), axis1.set_point, axis1.co, axis1.pos, axis1.vel, axis1.acc,*axis1.ptr_limit);
}
- /*
- this->set_point -= 1000;
- this->pid->setSetPoint(this->set_point);
+
+ axis2.set_point -= 1000;
+ axis2.pid->setSetPoint(axis2.set_point);
wait(.5);
- while(*this->ptr_limit == 1){
- this->set_point += 10;
- this->pid->setSetPoint(this->set_point);
- wait(.02);
- if(this->debug)
- printf("T=%.2f SP=%.3f co=%.3f pos=%.3f vel=%.3f acc=%.3f limit=%d\r\n", t.read(), this->set_point, this->co, this->pos, this->vel, this->acc,*this->ptr_limit);
- }*/
-
+ axis1.center();
}
void alive(void){
led1 = !led1;
if(led1)
- pulse.attach(&alive, .3); // the address of the function to be attached (flip) and the interval (2 seconds)
+ pulse.attach(&alive, .2); // the address of the function to be attached (flip) and the interval (2 seconds)
else
- pulse.attach(&alive, 1.5); // the address of the function to be attached (flip) and the interval (2 seconds)
+ pulse.attach(&alive, 1.3); // the address of the function to be attached (flip) and the interval (2 seconds)
+}
+
+int collisionCheck(void){
+ float current;
+ current = axis1.readCurrent();
+ pc.printf("\r\n%.3f ", current);
+ current = axis2.readCurrent();
+ pc.printf("%.3f ", current);
+ current = axis3.readCurrent();
+ pc.printf("%.3f ", current);
+ current = axis4.readCurrent();
+ pc.printf("%.3f ", current);
+ current = axis5.readCurrent();
+ pc.printf("%.3f ", current);
+ current = axis6.readCurrent();
+ pc.printf("%.3f\r\n", current);
+
+ return 1;
}
//------------------- MAIN --------------------------------
int main()
@@ -142,41 +154,46 @@
if(pcf.getError() != 0)
myerror(pcf.getErrorMessage());
- axis1.init(90.0/10680.0);
- axis2.init(180.0/20500.0);
+ axis1.init();
+ axis2.init();
+ axis3.init();
+ axis4.init();
+ axis5.init();
+ axis6.init();
+
axis1.debug = 1;
axis2.debug = 1;
+ axis3.debug = 1;
+ axis4.debug = 1;
+ axis5.debug = 1;
+ axis6.debug = 1;
t.start(); // Set up timer
-
- /*
- while(1){
- //axis1.motcon->mot_control(.5); //works
- pc.printf("%.2f,%.2f,%.2f,%.2f,%.2f,%.2f \r\n", axis1.pos, axis2.pos,axis3.pos,axis4.pos,axis5.pos,axis6.pos);
- wait(.2);
- }
- */
while(1){
if(pc.readable())
{
char c = pc.getc();
- if(c == 'A'){
+ if((c == 'A') || (c == 'a')){
home_test();
}
- if(c == '?') //move command
+ if(c == '?') //get commands
{
pc.printf("T - Trapezoidal Profile Move command\r\n");
- //pc.printf("M - Move command\r\n");
pc.printf("H- Home\r\n");
pc.printf("S- Set point in encoder counts\r\n");
- pc.printf("Z - Zero Encoders \r\n");
- pc.printf("X - Excercise Robotic Arm \r\n");
- pc.printf("spacebar - Emergency Stop \r\n");
+ pc.printf("Z - Zero Encoder\r\n");
+ pc.printf("X - Excercise Robotic Arm\r\n");
+ pc.printf("O - Axis to turn On (Default)\r\n");
+ pc.printf("F - Axis to turn Off \r\n");
+ pc.printf("P - Set Proportional Gain on an Axis\r\n");
+ pc.printf("I - Set Integral Gain on an Axis\r\n");
+ pc.printf("D - Set Derivative Gain on an Axis\r\n");
+ pc.printf("spacebar - Emergency Stop\r\n");
}
- if((c == 'X' || c == 'x')) //Excercise axes
+ if((c == 'X' || c == 'x')) //Excercise all axes
{
pc.printf("\r\nPress q to quit Excercise\r\n");
pc.printf("Received move test command\r\n");
@@ -222,8 +239,7 @@
}
}
}
- if(c == 'T' || c == 't') //move Trapazoid command
- {
+ if(c == 'T' || c == 't'){ //move Trapazoid command
float position = 0.0;
float time = 0.0;
@@ -270,8 +286,7 @@
break;
}
}
- if(c == 'H' || c == 'h')
- {
+ if(c == 'H' || c == 'h'){
pc.printf("Enter axis to home\r\n");
pc.scanf("%c",&c);
switch(c){
@@ -282,25 +297,44 @@
axis2.ls7366->LS7366_write_DTR(0);
axis2.enc = axis1.ls7366->LS7366_read_counter();
*/
- axis1.home(11000);
+ axis1.center();
break;
case '2':
- pc.printf("Homing Robotic Axis 2\r\n");
- axis2.home(6000);
+ pc.printf("Homing Robotic Axis 2\r\n");
+ axis2.center();
break;
+
+ case '3':
+ pc.printf("Homing Robotic Axis 3\r\n");
+ axis3.center();
+ break;
+
+ case '4':
+ pc.printf("Homing Robotic Axis 4\r\n");
+ axis4.center();
+ break;
+
+ case '5':
+ pc.printf("Homing Robotic Axis 5\r\n");
+ axis5.center();
+ break;
+
+ case '6':
+ pc.printf("Homing Robotic Axis 6\r\n");
+ axis6.center();
+ break;
}
}
- if(c == 'S' || c == 's')
- {
+ if(c == 'S' || c == 's'){
pc.printf("Enter axis to give set point\r\n");
- pc.scanf("%c",&c);
+ pc.scanf("%c",&c);
+ pc.printf("Enter value for set point axis %c\r\n", c);
switch(c){
- case '1':
- pc.printf("Enter value for set point axis 1\r\n");
+ case '1':
pc.scanf("%f",&axis1.set_point);
pc.printf("%f\r\n",axis1.set_point);
t.reset();
- while((axis1.enc > (axis1.set_point + 100)) || (axis1.enc < (axis1.set_point - 100))){
+ while((axis1.enc > (axis1.set_point + SP_TOL)) || (axis1.enc < (axis1.set_point - SP_TOL))){
pc.printf("T=%.2f SP=%.3f co=%.3f pos=%.3f vel=%.3f acc=%.3f \r\n", t.read(), axis1.set_point, axis1.co, axis1.pos, axis1.vel, axis1.acc);
wait(.009);
}
@@ -309,11 +343,10 @@
break;
case '2':
- pc.printf("Enter value for set point axis 2\r\n");
pc.scanf("%f",&axis2.set_point);
pc.printf("%f\r\n",axis2.set_point);
t.reset();
- while((axis2.enc > (axis2.set_point + 100)) || (axis2.enc < (axis2.set_point - 100))){
+ while((axis2.enc > (axis2.set_point + SP_TOL)) || (axis2.enc < (axis2.set_point - SP_TOL))){
pc.printf("T=%.2f SP=%.3f co=%.3f pos=%.3f vel=%.3f acc=%.3f \r\n", t.read(), axis2.set_point, axis2.co, axis2.pos, axis2.vel, axis2.acc);
wait(.009);
}
@@ -321,11 +354,10 @@
break;
case '3':
- pc.printf("Enter value for set point axis 3\r\n");
pc.scanf("%f",&axis3.set_point);
pc.printf("%f\r\n",axis3.set_point);
t.reset();
- while((axis3.enc > (axis3.set_point + 100)) || (axis3.enc < (axis3.set_point - 100))){
+ while((axis3.enc > (axis3.set_point + SP_TOL)) || (axis3.enc < (axis3.set_point - SP_TOL))){
pc.printf("T=%.2f SP=%.3f co=%.3f pos=%.3f vel=%.3f acc=%.3f \r\n", t.read(), axis3.set_point, axis3.co, axis3.pos, axis3.vel, axis3.acc);
wait(.009);
}
@@ -333,11 +365,10 @@
break;
case '4':
- pc.printf("Enter value for set point axis 4\r\n");
pc.scanf("%f",&axis4.set_point);
pc.printf("%f\r\n",axis4.set_point);
t.reset();
- while((axis4.enc > (axis4.set_point + 100)) || (axis4.enc < (axis4.set_point - 100))){
+ while((axis4.enc > (axis4.set_point + SP_TOL)) || (axis4.enc < (axis4.set_point - SP_TOL))){
pc.printf("T=%.2f SP=%.3f co=%.3f pos=%.3f vel=%.3f acc=%.3f \r\n", t.read(), axis4.set_point, axis4.co, axis4.pos, axis4.vel, axis4.acc);
wait(.009);
}
@@ -345,11 +376,10 @@
break;
case '5':
- pc.printf("Enter value for set point axis 5\r\n");
pc.scanf("%f",&axis5.set_point);
pc.printf("%f\r\n",axis5.set_point);
t.reset();
- while((axis5.enc > (axis5.set_point + 100)) || (axis5.enc < (axis5.set_point - 100))){
+ while((axis5.enc > (axis5.set_point + SP_TOL)) || (axis5.enc < (axis5.set_point - 50))){
pc.printf("T=%.2f SP=%.3f co=%.3f pos=%.3f vel=%.3f acc=%.3f \r\n", t.read(), axis5.set_point, axis5.co, axis5.pos, axis5.vel, axis5.acc);
wait(.009);
}
@@ -357,11 +387,10 @@
break;
case '6':
- pc.printf("Enter value for set point axis 6\r\n");
pc.scanf("%f",&axis6.set_point);
pc.printf("%f\r\n",axis6.set_point);
t.reset();
- while((axis6.enc > (axis6.set_point + 100)) || (axis6.enc < (axis6.set_point - 100))){
+ while((axis6.enc > (axis6.set_point + SP_TOL)) || (axis6.enc < (axis6.set_point - SP_TOL))){
pc.printf("T=%.2f SP=%.3f co=%.3f pos=%.3f vel=%.3f acc=%.3f \r\n", t.read(), axis6.set_point, axis6.co, axis6.pos, axis6.vel, axis6.acc);
wait(.009);
}
@@ -369,26 +398,112 @@
break;
}
}
- if(c == 'P' || c == 'p')
- {
- pc.printf("Enter value for Pk\r\n");
- pc.scanf("%f",&axis1.Pk);
- pc.printf("%f\r\n",axis1.Pk);
+ if(c == 'P' || c == 'p'){
+ float temp_Pk;
+ pc.printf("Enter axis to set Pk\r\n");
+ pc.scanf("%c",&c);
+
+ pc.printf("Enter value for Axis%c Pk\r\n", c);
+ pc.scanf("%f",&temp_Pk);
+
+ switch(c){
+ case 1:
+ axis1.Pk = temp_Pk;
+ break;
+
+ case 2:
+ axis2.Pk = temp_Pk;
+ break;
+
+ case 3:
+ axis3.Pk = temp_Pk;
+ break;
+
+ case 4:
+ axis4.Pk = temp_Pk;
+ break;
+
+ case 5:
+ axis5.Pk = temp_Pk;
+ break;
+
+ case 6:
+ axis6.Pk = temp_Pk;
+ break;
+ }
+ pc.printf("Axis%c Pk set to %f\r\n", c, temp_Pk);
}
- if(c == 'I' || c == 'i')
- {
- pc.printf("Enter value for Ik\r\n");
- pc.scanf("%f",&axis1.Ik);
- pc.printf("%f\r\n",axis1.Ik);
+ if(c == 'I' || c == 'i'){
+ float temp_Ik;
+ pc.printf("Enter axis to set Ik\r\n");
+ pc.scanf("%c",&c);
+
+ pc.printf("Enter value for Axis%c Ik\r\n", c);
+ pc.scanf("%f",&temp_Ik);
+
+ switch(c){
+ case 1:
+ axis1.Ik = temp_Ik;
+ break;
+
+ case 2:
+ axis2.Ik = temp_Ik;
+ break;
+
+ case 3:
+ axis3.Ik = temp_Ik;
+ break;
+
+ case 4:
+ axis4.Ik = temp_Ik;
+ break;
+
+ case 5:
+ axis5.Ik = temp_Ik;
+ break;
+
+ case 6:
+ axis6.Ik = temp_Ik;
+ break;
+ }
+ pc.printf("Axis%c Ik set to %f\r\n", c, temp_Ik);
}
- if(c == 'D' || c == 'd')
- {
- pc.printf("Enter value for Dk\r\n");
- pc.scanf("%f",&axis1.Dk);
- pc.printf("%f\r\n",axis1.Dk);
+ if(c == 'D' || c == 'd'){
+ float temp_Dk;
+ pc.printf("Enter axis to set Dk\r\n");
+ pc.scanf("%c",&c);
+
+ pc.printf("Enter value for Axis%c Dk\r\n", c);
+ pc.scanf("%f",&temp_Dk);
+
+ switch(c){
+ case 1:
+ axis1.Dk = temp_Dk;
+ break;
+
+ case 2:
+ axis2.Dk = temp_Dk;
+ break;
+
+ case 3:
+ axis3.Dk = temp_Dk;
+ break;
+
+ case 4:
+ axis4.Dk = temp_Dk;
+ break;
+
+ case 5:
+ axis5.Dk = temp_Dk;
+ break;
+
+ case 6:
+ axis6.Dk = temp_Dk;
+ break;
+ }
+ pc.printf("Axis%c Ik set to %f\r\n", c, temp_Dk);
}
- if(c == 'Z' || c == 'z')
- {
+ if(c == 'Z' || c == 'z'){
pc.printf("Enter axis to zero\r\n");
pc.scanf("%c",&c);
switch(c){
@@ -448,28 +563,107 @@
}
}
- if(c == ' ')
- {
- axis1.ls7366->LS7366_reset_counter();
- axis1.ls7366->LS7366_quad_mode_x4();
- axis1.ls7366->LS7366_write_DTR(0);
+ if(c == 'O' || c == 'o'){
+ pc.printf("Enter axis to turn On\r\n");
+ pc.scanf("%c",&c);
+
+ switch(c){
+ case '1':
+ axis1.axisOn();
+ break;
+
+ case '2':
+ axis2.axisOn();
+ break;
+
+ case '3':
+ axis3.axisOn();
+ break;
+
+ case '4':
+ axis4.axisOn();
+ break;
+
+ case '5':
+ axis5.axisOn();
+ break;
+
+ case '6':
+ axis6.axisOn();
+ break;
+ }
+ pc.printf("Axis%d On\r\n", c);
+ }
+ if(c == 'F' || c == 'f'){
+ pc.printf("Enter axis to turn Off\r\n");
+ pc.scanf("%c",&c);
- //read encoder values
- axis1.enc = axis1.ls7366->LS7366_read_counter();
- //long enc2 = LS7366_read_counter(2);
-
- //resets the controllers internals
- axis1.pid->reset();
- //start at 0
- axis1.set_point = 0.0;
- axis1.pid->setSetPoint(0);
+ switch(c){
+ case '1':
+ axis1.axisOff();
+ break;
+
+ case '2':
+ axis2.axisOff();
+ break;
+
+ case '3':
+ axis3.axisOff();
+ break;
+
+ case '4':
+ axis4.axisOff();
+ break;
+
+ case '5':
+ axis5.axisOff();
+ break;
+
+ case '6':
+ axis6.axisOff();
+ break;
+ }
+ pc.printf("Axis%d Off\r\n", c);
}
-
-//----- axis1.pid->setTunings(axis1.Pk, axis1.Ik, axis1.Dk);
- //T1_P = 0.0;
- //T1_I = 0.0; //reset integral
- //controller.reset();
- }
+ if(c == 'B' || c == 'b'){
+ pc.printf("Enter pitch counts\r\n");
+
+ float counts;
+
+ pc.scanf("%f",counts);
+ axis4.set_point = counts;
+ axis5.set_point = -counts;
+
+ pc.printf("%f\r\n",&counts);
+ t.reset();
+ while(axis4.enc > (axis4.set_point + SP_TOL) ||
+ axis4.enc < (axis4.set_point - SP_TOL) &&
+ axis5.enc > (axis5.set_point + SP_TOL) ||
+ axis5.enc < (axis5.set_point - SP_TOL)){
+ pc.printf("T=%.2f SP=%.3f co=%.3f pos=%.3f vel=%.3f acc=%.3f\r\n", t.read(), axis4.set_point, axis4.enc, axis5.set_point, axis5.enc);
+ wait(.009);
+ }
+ }
+ if(c == 'N' || c == 'n'){
+ pc.printf("Enter rotate counts\r\n");
+
+ float counts;
+
+ pc.scanf("%f",&counts);
+ axis4.set_point = counts;
+ axis5.set_point = counts;
+
+ pc.printf("%f\r\n",counts);
+ t.reset();
+ while(axis4.enc > (axis4.set_point + SP_TOL) ||
+ axis4.enc < (axis4.set_point - SP_TOL) &&
+ axis5.enc > (axis5.set_point + SP_TOL) ||
+ axis5.enc < (axis5.set_point - SP_TOL)){
+ pc.printf("T=%.2f SP=%.3f co=%.3f pos=%.3f vel=%.3f acc=%.3f \r\n", t.read(), axis4.set_point, axis4.enc, axis5.set_point, axis5.enc);
+ wait(.009);
+ }
+ }
+ }//command was received
if((axis1.moveState==0) && (axis2.moveState==0) && (axis3.moveState==0) && (axis4.moveState==0) && (axis5.moveState==0) && (axis6.moveState==0)){
pc.printf("%.2f,%.1f,%.1f,%.1f,%.1f,%.1f,%.1f,%.1f,%.1f,%.1f,%.1f,%.1f,%.1f\r\n", t.read(), axis1.set_point, axis1.pos, axis2.set_point, axis2.pos,
@@ -482,3 +676,4 @@
wait(.02);
}//while(1)
}//main
+