Revision 0:02d2a7571614, committed 2015-09-25

Comitter:

jebradshaw

Date:

Fri Sep 25 19:28:01 2015 +0000

Child:

1:0d7a1f813571

Commit message:

- LPC1768 working with 6 Axis objects declared

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Axis.lib	Fri Sep 25 19:28:01 2015 +0000
@@ -0,0 +1,1 @@
+Axis#cd249816dba8

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PCF8574.lib	Fri Sep 25 19:28:01 2015 +0000
@@ -0,0 +1,1 @@
+http://developer.mbed.org/users/bborredon/code/PCF8574/#6d75da25c179

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/lpc_axis.cpp	Fri Sep 25 19:28:01 2015 +0000
@@ -0,0 +1,484 @@
+// Test program for WSE-PROJ-SBC Scorbot Interface
+// J Bradshaw
+//lpc_axis.cpp
+
+// 20150731
+// 20150827 - Got Ticker Trapeziodal profile command working (moveTrapezoid, muveUpdate) 
+// 20150924 - Port from the mbed to the LPC1768 processor for the 6 axis robotic arm controller
+
+#include "mbed.h"
+#include "Axis.h"
+#include "stdlib.h"
+#include "PCF8574.h"    //library for the 
+
+#include <string>
+
+#define PI 3.14159
+#define PCF8574_ADDR 0    // I2c PCF8574 address is 0x00
+
+DigitalOut led1(P1_18); //blue
+DigitalOut led2(P1_20); //
+DigitalOut led3(P1_21);
+
+Serial pc(P0_2, P0_3);    //pc serial interface (USB)
+SPI spi(P0_9, P0_8, P0_7);        //MOSI, MISO, SCK
+
+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);
+
+PCF8574 pcf(P0_10,P0_11,PCF8574_ADDR,false);  // Declare PCF8574 i2c with sda and scl (p28,p27) (10K pullups!)
+//uint8_t data;
+Ticker pulse;
+
+static void myerror(std::string msg)
+{
+  printf("Error %s\n",msg.c_str());
+  exit(1);
+}
+
+void updateLimitSwitches(int state){
+    if((state & 0x01) == 0x01)
+        limit0 = 1;
+    else
+        limit0 = 0;
+        
+    if((state & 0x02) == 0x02)
+        limit1 = 1;   
+    else
+        limit1 = 0;
+
+    if((state & 0x04) == 0x04)
+        limit2 = 1;   
+    else
+        limit2 = 0;
+
+    if((state & 0x08) == 0x08)
+        limit3 = 1;   
+    else
+        limit3 = 0;
+        
+    if((state & 0x10) == 0x10)
+        limit4 = 1;   
+    else
+        limit4 = 0;
+
+    if((state & 0x20) == 0x20)
+        limit5 = 1;   
+    else
+        limit5 = 0;
+}
+    
+void pcf8574_it(uint8_t data, PCF8574 *o)
+{
+    int state;
+    state = pcf.read();
+    printf("PCF8574 interrupt data = %02x\n",state);
+    updateLimitSwitches(state);
+}
+
+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.set_point -= 100;
+       axis2.pid->setSetPoint(axis2.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);
+    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);
+    }*/
+    
+}
+
+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)     
+    else
+        pulse.attach(&alive, 1.5); // the address of the function to be attached (flip) and the interval (2 seconds)
+}
+//------------------- MAIN --------------------------------
+int main()
+{        
+    wait(.2);
+    pulse.attach(&alive, 2.0); // the address of the function to be attached (flip) and the interval (2 seconds)
+ 
+    pc.baud(921600);
+    //i2c.frequency(100000);
+    //LimitSwitch_0.mode(PullUp);  //set the mbed to use a pullup resistor
+    pc.printf("\r\n%s\r\n", __FILE__); //display the filename (this source file)
+    
+    // Set all IO port bits to 1 to enable inputs and test error
+    pcf = 0xff;
+    if(pcf.getError() != 0)
+        myerror(pcf.getErrorMessage());    
+    
+    // Assign interrupt function to pin P0_17 (mbed p12)
+    pcf.interrupt(P0_17,&pcf8574_it);
+    updateLimitSwitches(pcf.read());
+  
+    if(pcf.getError() != 0)
+        myerror(pcf.getErrorMessage());
+      
+    axis1.init(90.0/10680.0);
+    axis2.init(180.0/20500.0);
+    axis1.debug = 1;
+    axis2.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'){
+                home_test();
+            }
+            if(c == '?')    //move command
+            {
+                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");
+                
+            }
+            if((c == 'X' || c == 'x'))    //Excercise axes
+            {
+                pc.printf("\r\nPress q to quit Excercise\r\n");
+                pc.printf("Received move test command\r\n"); 
+                int qFlag=0;
+                float lastmovetime = 0.0;
+                t.reset();
+                while(qFlag==0){
+                    float movetime = rand() % 5;
+                    movetime += 1.0;
+                    lastmovetime = t.read() + movetime;
+                                                          
+                    axis1.moveTrapezoid((rand() % 7000) - 3500, movetime); 
+                    axis2.moveTrapezoid((rand() % 5000) - 2500, movetime);
+                    axis3.moveTrapezoid((rand() % 3000) - 1500, movetime); 
+                    axis4.moveTrapezoid((rand() % 3000) - 1500, movetime);
+                    axis5.moveTrapezoid((rand() % 3000) - 1500, movetime); 
+                    axis6.moveTrapezoid((rand() % 3000) - 1500, movetime);                    
+                    
+                    while(t.read() <= lastmovetime + 1.0){
+                        //pc.printf("T=%.2f ax1SP=%.3f ax1pos=%.3f ax2SP=%.3f ax2pos=%.3f \r\n", t.read(), axis1.set_point, axis1.pos, axis2.set_point, axis2.pos);
+                        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,
+                            axis3.set_point, axis3.pos, axis4.set_point, axis4.pos,axis5.set_point, axis5.pos, axis6.set_point, axis6.pos);
+                        wait(.01);
+                        
+                        if(pc.readable()){      //if user types a 'q' or 'Q'
+                            char c = pc.getc();
+                            if(c == 'q' || c == 'Q') //quit after current movement
+                                qFlag = 1;         //set the flag to terminate the excercise
+                            if(c == ' '){           //stop immediately
+                                axis1.moveState = 4;
+                                axis2.moveState = 4;
+                                axis3.moveState = 4;
+                                axis4.moveState = 4;
+                                axis5.moveState = 4;
+                                axis6.moveState = 4;
+                                qFlag = 1;         //set the flag to terminate the excercise
+                                break;
+                            }                                
+                        }                                                
+                    }                    
+                    if(t.read() < 0.0){          //if time goes negative, reset the timer
+                        t.reset();
+                    }
+                }
+            }
+            if(c == 'T' || c == 't')    //move Trapazoid command
+            {
+                float position = 0.0;
+                float time = 0.0;
+                
+                pc.printf("Enter axis to move trapazoid\r\n");        
+                pc.scanf("%c",&c);
+                
+                pc.printf("\r\n\r\nEnter position:");        
+                pc.scanf("%f",&position);
+                pc.printf("%f\r\n", position); 
+                
+                pc.printf("Enter time:");        
+                pc.scanf("%f",&time);
+                pc.printf("%f\r\n", time);
+                                          
+                switch(c){
+                    case '1':
+                        pc.printf("Moving Robotic Axis 1\r\n");        
+                        axis1.moveTrapezoid(position, time);       
+                    break; 
+                    
+                    case '2':
+                        pc.printf("Moving Robotic Axis 2\r\n");        
+                        axis2.moveTrapezoid(position, time);           
+                    break;   
+                    
+                    case '3':
+                        pc.printf("Moving Robotic Axis 3\r\n");        
+                        axis3.moveTrapezoid(position, time);           
+                    break;
+                    
+                    case '4':
+                        pc.printf("Moving Robotic Axis 4\r\n");        
+                        axis4.moveTrapezoid(position, time);           
+                    break;   
+                    
+                    case '5':
+                        pc.printf("Moving Robotic Axis 5\r\n");        
+                        axis5.moveTrapezoid(position, time);           
+                    break;
+
+                    case '6':
+                        pc.printf("Moving Robotic Axis 6\r\n");        
+                        axis6.moveTrapezoid(position, time);           
+                    break;
+                }                               
+            }            
+            if(c == 'H' || c == 'h')
+            {
+                pc.printf("Enter axis to home\r\n");        
+                pc.scanf("%c",&c);          
+                switch(c){
+                    case '1':
+                        pc.printf("Homing Robotic Axis 1\r\n");
+/*                        axis2.ls7366->LS7366_reset_counter();
+                        axis2.ls7366->LS7366_quad_mode_x4();   
+                        axis2.ls7366->LS7366_write_DTR(0);
+                        axis2.enc = axis1.ls7366->LS7366_read_counter();        
+*/                        
+                        axis1.home(11000);           
+                    break; 
+                    case '2':
+                        pc.printf("Homing Robotic Axis 2\r\n");        
+                        axis2.home(6000);           
+                    break;   
+                }
+            }                        
+            if(c == 'S' || c == 's')
+            {
+                pc.printf("Enter axis to give set point\r\n");        
+                pc.scanf("%c",&c);          
+                switch(c){                                        
+                    case '1':
+                        pc.printf("Enter value for set point axis 1\r\n");        
+                        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))){
+                            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);
+                        }
+                        pc.printf("T=%.2f SP=%.3f co=%.3f pos=%.3f vel=%.3f acc=%.3f       \r\n\n", t.read(), axis1.set_point, axis1.co, axis1.pos, axis1.vel, axis1.acc);           
+
+                    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))){
+                            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);
+                        }
+                        pc.printf("T=%.2f SP=%.3f co=%.3f pos=%.3f vel=%.3f acc=%.3f       \r\n\n", t.read(), axis2.set_point, axis2.co, axis2.pos, axis2.vel, axis2.acc);           
+                    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))){
+                            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);
+                        }
+                        pc.printf("T=%.2f SP=%.3f co=%.3f pos=%.3f vel=%.3f acc=%.3f       \r\n\n", t.read(), axis3.set_point, axis3.co, axis3.pos, axis3.vel, axis3.acc);           
+                    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))){
+                            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);
+                        }
+                        pc.printf("T=%.2f SP=%.3f co=%.3f pos=%.3f vel=%.3f acc=%.3f       \r\n\n", t.read(), axis4.set_point, axis4.co, axis4.pos, axis4.vel, axis4.acc);           
+                    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))){
+                            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);
+                        }
+                        pc.printf("T=%.2f SP=%.3f co=%.3f pos=%.3f vel=%.3f acc=%.3f       \r\n\n", t.read(), axis5.set_point, axis5.co, axis5.pos, axis5.vel, axis5.acc);           
+                    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))){
+                            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);
+                        }
+                        pc.printf("T=%.2f SP=%.3f co=%.3f pos=%.3f vel=%.3f acc=%.3f       \r\n\n", t.read(), axis6.set_point, axis6.co, axis6.pos, axis6.vel, axis6.acc);           
+                    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 == '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 == '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 == 'Z' || c == 'z')
+            {
+                pc.printf("Enter axis to zero\r\n");        
+                pc.scanf("%c",&c);          
+                switch(c){                                        
+                    case '1':
+                        axis1.ls7366->LS7366_reset_counter();
+                        axis1.ls7366->LS7366_quad_mode_x4();       
+                        axis1.ls7366->LS7366_write_DTR(0);
+                        
+                        axis1.set_point = 0.0;
+                        axis1.pid->setSetPoint(0);
+                    break;
+
+                    case '2':
+                        axis2.ls7366->LS7366_reset_counter();
+                        axis2.ls7366->LS7366_quad_mode_x4();       
+                        axis2.ls7366->LS7366_write_DTR(0);
+                        
+                        axis2.set_point = 0.0;
+                        axis2.pid->setSetPoint(0);
+                    break;
+                    
+                    case '3':
+                        axis3.ls7366->LS7366_reset_counter();
+                        axis3.ls7366->LS7366_quad_mode_x4();       
+                        axis3.ls7366->LS7366_write_DTR(0);
+                        
+                        axis3.set_point = 0.0;
+                        axis3.pid->setSetPoint(0);
+                    break;
+                    
+                    case '4':
+                        axis4.ls7366->LS7366_reset_counter();
+                        axis4.ls7366->LS7366_quad_mode_x4();       
+                        axis4.ls7366->LS7366_write_DTR(0);
+                        
+                        axis4.set_point = 0.0;
+                        axis4.pid->setSetPoint(0);
+                    break;
+                    
+                    case '5':
+                        axis5.ls7366->LS7366_reset_counter();
+                        axis5.ls7366->LS7366_quad_mode_x4();       
+                        axis5.ls7366->LS7366_write_DTR(0);
+                        
+                        axis5.set_point = 0.0;
+                        axis5.pid->setSetPoint(0);                        
+                    break;
+                    
+                    case '6':
+                        axis6.ls7366->LS7366_reset_counter();
+                        axis6.ls7366->LS7366_quad_mode_x4();       
+                        axis6.ls7366->LS7366_write_DTR(0);
+                        
+                        axis6.set_point = 0.0;
+                        axis6.pid->setSetPoint(0);
+                    break;                                                                                                    
+                }
+                    
+            }            
+            if(c == ' ')
+            {
+                axis1.ls7366->LS7366_reset_counter();
+                axis1.ls7366->LS7366_quad_mode_x4();       
+                axis1.ls7366->LS7366_write_DTR(0);                
+                
+                //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);                
+            }
+  
+//-----            axis1.pid->setTunings(axis1.Pk, axis1.Ik, axis1.Dk);
+            //T1_P = 0.0;
+            //T1_I = 0.0; //reset integral    
+            //controller.reset();   
+        }
+                                                   
+        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,
+                axis3.set_point, axis3.pos, axis4.set_point, axis4.pos,axis5.set_point, axis5.pos, axis6.set_point, axis6.pos);                
+            led2 = 0;
+        }
+        else
+            led2 = 1;
+                                                    
+        wait(.02);
+    }//while(1)                        
+}//main

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Fri Sep 25 19:28:01 2015 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/4f6c30876dfa
\ No newline at end of file