Serial interface for controlling robotic arm.

Dependencies:   Axis mbed

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.

/media/uploads/jebradshaw/axiscontroller_protoboardsmall.jpg

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!!

/media/uploads/jebradshaw/pcb_artwork.jpg Express PCB Artwork

/media/uploads/jebradshaw/6axiscontroller_innerpowerlayer.jpg Inner Power Layer Breakup for motor current

/media/uploads/jebradshaw/6axiscontroller_pcb_prototype_trimmed.jpg First Prototype

/media/uploads/jebradshaw/lpc1768_axiscontroller_part1_20161214.bmp

/media/uploads/jebradshaw/lpc1768_axiscontroller_part2_20161216.bmp

/media/uploads/jebradshaw/lpc1768_axiscontroller_part3_20161216.bmp

/media/uploads/jebradshaw/lpc1768_axiscontroller_part4_20190124_2.jpg

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 /media/uploads/jebradshaw/silkscreen_top_stencil.jpg /media/uploads/jebradshaw/axiscontroller_bottommirrorimage_20161216.jpg

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

Committer:
jebradshaw
Date:
Fri Sep 25 19:28:01 2015 +0000
Revision:
0:02d2a7571614
Child:
1:0d7a1f813571
- LPC1768 working with 6 Axis objects declared

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jebradshaw 0:02d2a7571614 1 // Test program for WSE-PROJ-SBC Scorbot Interface
jebradshaw 0:02d2a7571614 2 // J Bradshaw
jebradshaw 0:02d2a7571614 3 //lpc_axis.cpp
jebradshaw 0:02d2a7571614 4
jebradshaw 0:02d2a7571614 5 // 20150731
jebradshaw 0:02d2a7571614 6 // 20150827 - Got Ticker Trapeziodal profile command working (moveTrapezoid, muveUpdate)
jebradshaw 0:02d2a7571614 7 // 20150924 - Port from the mbed to the LPC1768 processor for the 6 axis robotic arm controller
jebradshaw 0:02d2a7571614 8
jebradshaw 0:02d2a7571614 9 #include "mbed.h"
jebradshaw 0:02d2a7571614 10 #include "Axis.h"
jebradshaw 0:02d2a7571614 11 #include "stdlib.h"
jebradshaw 0:02d2a7571614 12 #include "PCF8574.h" //library for the
jebradshaw 0:02d2a7571614 13
jebradshaw 0:02d2a7571614 14 #include <string>
jebradshaw 0:02d2a7571614 15
jebradshaw 0:02d2a7571614 16 #define PI 3.14159
jebradshaw 0:02d2a7571614 17 #define PCF8574_ADDR 0 // I2c PCF8574 address is 0x00
jebradshaw 0:02d2a7571614 18
jebradshaw 0:02d2a7571614 19 DigitalOut led1(P1_18); //blue
jebradshaw 0:02d2a7571614 20 DigitalOut led2(P1_20); //
jebradshaw 0:02d2a7571614 21 DigitalOut led3(P1_21);
jebradshaw 0:02d2a7571614 22
jebradshaw 0:02d2a7571614 23 Serial pc(P0_2, P0_3); //pc serial interface (USB)
jebradshaw 0:02d2a7571614 24 SPI spi(P0_9, P0_8, P0_7); //MOSI, MISO, SCK
jebradshaw 0:02d2a7571614 25
jebradshaw 0:02d2a7571614 26 int limit0, limit1, limit2, limit3, limit4, limit5; //global limit switch values
jebradshaw 0:02d2a7571614 27
jebradshaw 0:02d2a7571614 28 Timer t;
jebradshaw 0:02d2a7571614 29 //instantiate axis object
jebradshaw 0:02d2a7571614 30 Axis axis1(spi, P1_24, P2_5, P1_0, &limit0); //spi bus, LS7366_cs, pwm, dir, limitSwitch
jebradshaw 0:02d2a7571614 31 Axis axis2(spi, P1_25, P2_4, P1_1, &limit1);
jebradshaw 0:02d2a7571614 32 Axis axis3(spi, P1_26, P2_3, P1_4, &limit2);
jebradshaw 0:02d2a7571614 33 Axis axis4(spi, P1_27, P2_2, P1_8, &limit3);
jebradshaw 0:02d2a7571614 34 Axis axis5(spi, P1_28, P2_1, P1_9, &limit4);
jebradshaw 0:02d2a7571614 35 Axis axis6(spi, P1_29, P2_0, P1_10, &limit5);
jebradshaw 0:02d2a7571614 36
jebradshaw 0:02d2a7571614 37 PCF8574 pcf(P0_10,P0_11,PCF8574_ADDR,false); // Declare PCF8574 i2c with sda and scl (p28,p27) (10K pullups!)
jebradshaw 0:02d2a7571614 38 //uint8_t data;
jebradshaw 0:02d2a7571614 39 Ticker pulse;
jebradshaw 0:02d2a7571614 40
jebradshaw 0:02d2a7571614 41 static void myerror(std::string msg)
jebradshaw 0:02d2a7571614 42 {
jebradshaw 0:02d2a7571614 43 printf("Error %s\n",msg.c_str());
jebradshaw 0:02d2a7571614 44 exit(1);
jebradshaw 0:02d2a7571614 45 }
jebradshaw 0:02d2a7571614 46
jebradshaw 0:02d2a7571614 47 void updateLimitSwitches(int state){
jebradshaw 0:02d2a7571614 48 if((state & 0x01) == 0x01)
jebradshaw 0:02d2a7571614 49 limit0 = 1;
jebradshaw 0:02d2a7571614 50 else
jebradshaw 0:02d2a7571614 51 limit0 = 0;
jebradshaw 0:02d2a7571614 52
jebradshaw 0:02d2a7571614 53 if((state & 0x02) == 0x02)
jebradshaw 0:02d2a7571614 54 limit1 = 1;
jebradshaw 0:02d2a7571614 55 else
jebradshaw 0:02d2a7571614 56 limit1 = 0;
jebradshaw 0:02d2a7571614 57
jebradshaw 0:02d2a7571614 58 if((state & 0x04) == 0x04)
jebradshaw 0:02d2a7571614 59 limit2 = 1;
jebradshaw 0:02d2a7571614 60 else
jebradshaw 0:02d2a7571614 61 limit2 = 0;
jebradshaw 0:02d2a7571614 62
jebradshaw 0:02d2a7571614 63 if((state & 0x08) == 0x08)
jebradshaw 0:02d2a7571614 64 limit3 = 1;
jebradshaw 0:02d2a7571614 65 else
jebradshaw 0:02d2a7571614 66 limit3 = 0;
jebradshaw 0:02d2a7571614 67
jebradshaw 0:02d2a7571614 68 if((state & 0x10) == 0x10)
jebradshaw 0:02d2a7571614 69 limit4 = 1;
jebradshaw 0:02d2a7571614 70 else
jebradshaw 0:02d2a7571614 71 limit4 = 0;
jebradshaw 0:02d2a7571614 72
jebradshaw 0:02d2a7571614 73 if((state & 0x20) == 0x20)
jebradshaw 0:02d2a7571614 74 limit5 = 1;
jebradshaw 0:02d2a7571614 75 else
jebradshaw 0:02d2a7571614 76 limit5 = 0;
jebradshaw 0:02d2a7571614 77 }
jebradshaw 0:02d2a7571614 78
jebradshaw 0:02d2a7571614 79 void pcf8574_it(uint8_t data, PCF8574 *o)
jebradshaw 0:02d2a7571614 80 {
jebradshaw 0:02d2a7571614 81 int state;
jebradshaw 0:02d2a7571614 82 state = pcf.read();
jebradshaw 0:02d2a7571614 83 printf("PCF8574 interrupt data = %02x\n",state);
jebradshaw 0:02d2a7571614 84 updateLimitSwitches(state);
jebradshaw 0:02d2a7571614 85 }
jebradshaw 0:02d2a7571614 86
jebradshaw 0:02d2a7571614 87 void home_test(void){
jebradshaw 0:02d2a7571614 88 axis1.pid->setInputLimits(-20000.0, 20000.0);
jebradshaw 0:02d2a7571614 89 while(*axis1.ptr_limit == 1){
jebradshaw 0:02d2a7571614 90 axis1.set_point += 100;
jebradshaw 0:02d2a7571614 91 axis1.pid->setSetPoint(axis1.set_point);
jebradshaw 0:02d2a7571614 92
jebradshaw 0:02d2a7571614 93 axis2.set_point -= 100;
jebradshaw 0:02d2a7571614 94 axis2.pid->setSetPoint(axis2.set_point);
jebradshaw 0:02d2a7571614 95
jebradshaw 0:02d2a7571614 96 wait(.05);
jebradshaw 0:02d2a7571614 97 if(axis1.debug)
jebradshaw 0:02d2a7571614 98 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);
jebradshaw 0:02d2a7571614 99 }
jebradshaw 0:02d2a7571614 100 /*
jebradshaw 0:02d2a7571614 101 this->set_point -= 1000;
jebradshaw 0:02d2a7571614 102 this->pid->setSetPoint(this->set_point);
jebradshaw 0:02d2a7571614 103 wait(.5);
jebradshaw 0:02d2a7571614 104
jebradshaw 0:02d2a7571614 105 while(*this->ptr_limit == 1){
jebradshaw 0:02d2a7571614 106 this->set_point += 10;
jebradshaw 0:02d2a7571614 107 this->pid->setSetPoint(this->set_point);
jebradshaw 0:02d2a7571614 108 wait(.02);
jebradshaw 0:02d2a7571614 109 if(this->debug)
jebradshaw 0:02d2a7571614 110 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);
jebradshaw 0:02d2a7571614 111 }*/
jebradshaw 0:02d2a7571614 112
jebradshaw 0:02d2a7571614 113 }
jebradshaw 0:02d2a7571614 114
jebradshaw 0:02d2a7571614 115 void alive(void){
jebradshaw 0:02d2a7571614 116 led1 = !led1;
jebradshaw 0:02d2a7571614 117 if(led1)
jebradshaw 0:02d2a7571614 118 pulse.attach(&alive, .3); // the address of the function to be attached (flip) and the interval (2 seconds)
jebradshaw 0:02d2a7571614 119 else
jebradshaw 0:02d2a7571614 120 pulse.attach(&alive, 1.5); // the address of the function to be attached (flip) and the interval (2 seconds)
jebradshaw 0:02d2a7571614 121 }
jebradshaw 0:02d2a7571614 122 //------------------- MAIN --------------------------------
jebradshaw 0:02d2a7571614 123 int main()
jebradshaw 0:02d2a7571614 124 {
jebradshaw 0:02d2a7571614 125 wait(.2);
jebradshaw 0:02d2a7571614 126 pulse.attach(&alive, 2.0); // the address of the function to be attached (flip) and the interval (2 seconds)
jebradshaw 0:02d2a7571614 127
jebradshaw 0:02d2a7571614 128 pc.baud(921600);
jebradshaw 0:02d2a7571614 129 //i2c.frequency(100000);
jebradshaw 0:02d2a7571614 130 //LimitSwitch_0.mode(PullUp); //set the mbed to use a pullup resistor
jebradshaw 0:02d2a7571614 131 pc.printf("\r\n%s\r\n", __FILE__); //display the filename (this source file)
jebradshaw 0:02d2a7571614 132
jebradshaw 0:02d2a7571614 133 // Set all IO port bits to 1 to enable inputs and test error
jebradshaw 0:02d2a7571614 134 pcf = 0xff;
jebradshaw 0:02d2a7571614 135 if(pcf.getError() != 0)
jebradshaw 0:02d2a7571614 136 myerror(pcf.getErrorMessage());
jebradshaw 0:02d2a7571614 137
jebradshaw 0:02d2a7571614 138 // Assign interrupt function to pin P0_17 (mbed p12)
jebradshaw 0:02d2a7571614 139 pcf.interrupt(P0_17,&pcf8574_it);
jebradshaw 0:02d2a7571614 140 updateLimitSwitches(pcf.read());
jebradshaw 0:02d2a7571614 141
jebradshaw 0:02d2a7571614 142 if(pcf.getError() != 0)
jebradshaw 0:02d2a7571614 143 myerror(pcf.getErrorMessage());
jebradshaw 0:02d2a7571614 144
jebradshaw 0:02d2a7571614 145 axis1.init(90.0/10680.0);
jebradshaw 0:02d2a7571614 146 axis2.init(180.0/20500.0);
jebradshaw 0:02d2a7571614 147 axis1.debug = 1;
jebradshaw 0:02d2a7571614 148 axis2.debug = 1;
jebradshaw 0:02d2a7571614 149
jebradshaw 0:02d2a7571614 150 t.start(); // Set up timer
jebradshaw 0:02d2a7571614 151
jebradshaw 0:02d2a7571614 152 /*
jebradshaw 0:02d2a7571614 153 while(1){
jebradshaw 0:02d2a7571614 154 //axis1.motcon->mot_control(.5); //works
jebradshaw 0:02d2a7571614 155 pc.printf("%.2f,%.2f,%.2f,%.2f,%.2f,%.2f \r\n", axis1.pos, axis2.pos,axis3.pos,axis4.pos,axis5.pos,axis6.pos);
jebradshaw 0:02d2a7571614 156 wait(.2);
jebradshaw 0:02d2a7571614 157 }
jebradshaw 0:02d2a7571614 158 */
jebradshaw 0:02d2a7571614 159
jebradshaw 0:02d2a7571614 160 while(1){
jebradshaw 0:02d2a7571614 161
jebradshaw 0:02d2a7571614 162 if(pc.readable())
jebradshaw 0:02d2a7571614 163 {
jebradshaw 0:02d2a7571614 164 char c = pc.getc();
jebradshaw 0:02d2a7571614 165 if(c == 'A'){
jebradshaw 0:02d2a7571614 166 home_test();
jebradshaw 0:02d2a7571614 167 }
jebradshaw 0:02d2a7571614 168 if(c == '?') //move command
jebradshaw 0:02d2a7571614 169 {
jebradshaw 0:02d2a7571614 170 pc.printf("T - Trapezoidal Profile Move command\r\n");
jebradshaw 0:02d2a7571614 171 //pc.printf("M - Move command\r\n");
jebradshaw 0:02d2a7571614 172 pc.printf("H- Home\r\n");
jebradshaw 0:02d2a7571614 173 pc.printf("S- Set point in encoder counts\r\n");
jebradshaw 0:02d2a7571614 174 pc.printf("Z - Zero Encoders \r\n");
jebradshaw 0:02d2a7571614 175 pc.printf("X - Excercise Robotic Arm \r\n");
jebradshaw 0:02d2a7571614 176 pc.printf("spacebar - Emergency Stop \r\n");
jebradshaw 0:02d2a7571614 177
jebradshaw 0:02d2a7571614 178 }
jebradshaw 0:02d2a7571614 179 if((c == 'X' || c == 'x')) //Excercise axes
jebradshaw 0:02d2a7571614 180 {
jebradshaw 0:02d2a7571614 181 pc.printf("\r\nPress q to quit Excercise\r\n");
jebradshaw 0:02d2a7571614 182 pc.printf("Received move test command\r\n");
jebradshaw 0:02d2a7571614 183 int qFlag=0;
jebradshaw 0:02d2a7571614 184 float lastmovetime = 0.0;
jebradshaw 0:02d2a7571614 185 t.reset();
jebradshaw 0:02d2a7571614 186 while(qFlag==0){
jebradshaw 0:02d2a7571614 187 float movetime = rand() % 5;
jebradshaw 0:02d2a7571614 188 movetime += 1.0;
jebradshaw 0:02d2a7571614 189 lastmovetime = t.read() + movetime;
jebradshaw 0:02d2a7571614 190
jebradshaw 0:02d2a7571614 191 axis1.moveTrapezoid((rand() % 7000) - 3500, movetime);
jebradshaw 0:02d2a7571614 192 axis2.moveTrapezoid((rand() % 5000) - 2500, movetime);
jebradshaw 0:02d2a7571614 193 axis3.moveTrapezoid((rand() % 3000) - 1500, movetime);
jebradshaw 0:02d2a7571614 194 axis4.moveTrapezoid((rand() % 3000) - 1500, movetime);
jebradshaw 0:02d2a7571614 195 axis5.moveTrapezoid((rand() % 3000) - 1500, movetime);
jebradshaw 0:02d2a7571614 196 axis6.moveTrapezoid((rand() % 3000) - 1500, movetime);
jebradshaw 0:02d2a7571614 197
jebradshaw 0:02d2a7571614 198 while(t.read() <= lastmovetime + 1.0){
jebradshaw 0:02d2a7571614 199 //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);
jebradshaw 0:02d2a7571614 200 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,
jebradshaw 0:02d2a7571614 201 axis3.set_point, axis3.pos, axis4.set_point, axis4.pos,axis5.set_point, axis5.pos, axis6.set_point, axis6.pos);
jebradshaw 0:02d2a7571614 202 wait(.01);
jebradshaw 0:02d2a7571614 203
jebradshaw 0:02d2a7571614 204 if(pc.readable()){ //if user types a 'q' or 'Q'
jebradshaw 0:02d2a7571614 205 char c = pc.getc();
jebradshaw 0:02d2a7571614 206 if(c == 'q' || c == 'Q') //quit after current movement
jebradshaw 0:02d2a7571614 207 qFlag = 1; //set the flag to terminate the excercise
jebradshaw 0:02d2a7571614 208 if(c == ' '){ //stop immediately
jebradshaw 0:02d2a7571614 209 axis1.moveState = 4;
jebradshaw 0:02d2a7571614 210 axis2.moveState = 4;
jebradshaw 0:02d2a7571614 211 axis3.moveState = 4;
jebradshaw 0:02d2a7571614 212 axis4.moveState = 4;
jebradshaw 0:02d2a7571614 213 axis5.moveState = 4;
jebradshaw 0:02d2a7571614 214 axis6.moveState = 4;
jebradshaw 0:02d2a7571614 215 qFlag = 1; //set the flag to terminate the excercise
jebradshaw 0:02d2a7571614 216 break;
jebradshaw 0:02d2a7571614 217 }
jebradshaw 0:02d2a7571614 218 }
jebradshaw 0:02d2a7571614 219 }
jebradshaw 0:02d2a7571614 220 if(t.read() < 0.0){ //if time goes negative, reset the timer
jebradshaw 0:02d2a7571614 221 t.reset();
jebradshaw 0:02d2a7571614 222 }
jebradshaw 0:02d2a7571614 223 }
jebradshaw 0:02d2a7571614 224 }
jebradshaw 0:02d2a7571614 225 if(c == 'T' || c == 't') //move Trapazoid command
jebradshaw 0:02d2a7571614 226 {
jebradshaw 0:02d2a7571614 227 float position = 0.0;
jebradshaw 0:02d2a7571614 228 float time = 0.0;
jebradshaw 0:02d2a7571614 229
jebradshaw 0:02d2a7571614 230 pc.printf("Enter axis to move trapazoid\r\n");
jebradshaw 0:02d2a7571614 231 pc.scanf("%c",&c);
jebradshaw 0:02d2a7571614 232
jebradshaw 0:02d2a7571614 233 pc.printf("\r\n\r\nEnter position:");
jebradshaw 0:02d2a7571614 234 pc.scanf("%f",&position);
jebradshaw 0:02d2a7571614 235 pc.printf("%f\r\n", position);
jebradshaw 0:02d2a7571614 236
jebradshaw 0:02d2a7571614 237 pc.printf("Enter time:");
jebradshaw 0:02d2a7571614 238 pc.scanf("%f",&time);
jebradshaw 0:02d2a7571614 239 pc.printf("%f\r\n", time);
jebradshaw 0:02d2a7571614 240
jebradshaw 0:02d2a7571614 241 switch(c){
jebradshaw 0:02d2a7571614 242 case '1':
jebradshaw 0:02d2a7571614 243 pc.printf("Moving Robotic Axis 1\r\n");
jebradshaw 0:02d2a7571614 244 axis1.moveTrapezoid(position, time);
jebradshaw 0:02d2a7571614 245 break;
jebradshaw 0:02d2a7571614 246
jebradshaw 0:02d2a7571614 247 case '2':
jebradshaw 0:02d2a7571614 248 pc.printf("Moving Robotic Axis 2\r\n");
jebradshaw 0:02d2a7571614 249 axis2.moveTrapezoid(position, time);
jebradshaw 0:02d2a7571614 250 break;
jebradshaw 0:02d2a7571614 251
jebradshaw 0:02d2a7571614 252 case '3':
jebradshaw 0:02d2a7571614 253 pc.printf("Moving Robotic Axis 3\r\n");
jebradshaw 0:02d2a7571614 254 axis3.moveTrapezoid(position, time);
jebradshaw 0:02d2a7571614 255 break;
jebradshaw 0:02d2a7571614 256
jebradshaw 0:02d2a7571614 257 case '4':
jebradshaw 0:02d2a7571614 258 pc.printf("Moving Robotic Axis 4\r\n");
jebradshaw 0:02d2a7571614 259 axis4.moveTrapezoid(position, time);
jebradshaw 0:02d2a7571614 260 break;
jebradshaw 0:02d2a7571614 261
jebradshaw 0:02d2a7571614 262 case '5':
jebradshaw 0:02d2a7571614 263 pc.printf("Moving Robotic Axis 5\r\n");
jebradshaw 0:02d2a7571614 264 axis5.moveTrapezoid(position, time);
jebradshaw 0:02d2a7571614 265 break;
jebradshaw 0:02d2a7571614 266
jebradshaw 0:02d2a7571614 267 case '6':
jebradshaw 0:02d2a7571614 268 pc.printf("Moving Robotic Axis 6\r\n");
jebradshaw 0:02d2a7571614 269 axis6.moveTrapezoid(position, time);
jebradshaw 0:02d2a7571614 270 break;
jebradshaw 0:02d2a7571614 271 }
jebradshaw 0:02d2a7571614 272 }
jebradshaw 0:02d2a7571614 273 if(c == 'H' || c == 'h')
jebradshaw 0:02d2a7571614 274 {
jebradshaw 0:02d2a7571614 275 pc.printf("Enter axis to home\r\n");
jebradshaw 0:02d2a7571614 276 pc.scanf("%c",&c);
jebradshaw 0:02d2a7571614 277 switch(c){
jebradshaw 0:02d2a7571614 278 case '1':
jebradshaw 0:02d2a7571614 279 pc.printf("Homing Robotic Axis 1\r\n");
jebradshaw 0:02d2a7571614 280 /* axis2.ls7366->LS7366_reset_counter();
jebradshaw 0:02d2a7571614 281 axis2.ls7366->LS7366_quad_mode_x4();
jebradshaw 0:02d2a7571614 282 axis2.ls7366->LS7366_write_DTR(0);
jebradshaw 0:02d2a7571614 283 axis2.enc = axis1.ls7366->LS7366_read_counter();
jebradshaw 0:02d2a7571614 284 */
jebradshaw 0:02d2a7571614 285 axis1.home(11000);
jebradshaw 0:02d2a7571614 286 break;
jebradshaw 0:02d2a7571614 287 case '2':
jebradshaw 0:02d2a7571614 288 pc.printf("Homing Robotic Axis 2\r\n");
jebradshaw 0:02d2a7571614 289 axis2.home(6000);
jebradshaw 0:02d2a7571614 290 break;
jebradshaw 0:02d2a7571614 291 }
jebradshaw 0:02d2a7571614 292 }
jebradshaw 0:02d2a7571614 293 if(c == 'S' || c == 's')
jebradshaw 0:02d2a7571614 294 {
jebradshaw 0:02d2a7571614 295 pc.printf("Enter axis to give set point\r\n");
jebradshaw 0:02d2a7571614 296 pc.scanf("%c",&c);
jebradshaw 0:02d2a7571614 297 switch(c){
jebradshaw 0:02d2a7571614 298 case '1':
jebradshaw 0:02d2a7571614 299 pc.printf("Enter value for set point axis 1\r\n");
jebradshaw 0:02d2a7571614 300 pc.scanf("%f",&axis1.set_point);
jebradshaw 0:02d2a7571614 301 pc.printf("%f\r\n",axis1.set_point);
jebradshaw 0:02d2a7571614 302 t.reset();
jebradshaw 0:02d2a7571614 303 while((axis1.enc > (axis1.set_point + 100)) || (axis1.enc < (axis1.set_point - 100))){
jebradshaw 0:02d2a7571614 304 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);
jebradshaw 0:02d2a7571614 305 wait(.009);
jebradshaw 0:02d2a7571614 306 }
jebradshaw 0:02d2a7571614 307 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);
jebradshaw 0:02d2a7571614 308
jebradshaw 0:02d2a7571614 309 break;
jebradshaw 0:02d2a7571614 310
jebradshaw 0:02d2a7571614 311 case '2':
jebradshaw 0:02d2a7571614 312 pc.printf("Enter value for set point axis 2\r\n");
jebradshaw 0:02d2a7571614 313 pc.scanf("%f",&axis2.set_point);
jebradshaw 0:02d2a7571614 314 pc.printf("%f\r\n",axis2.set_point);
jebradshaw 0:02d2a7571614 315 t.reset();
jebradshaw 0:02d2a7571614 316 while((axis2.enc > (axis2.set_point + 100)) || (axis2.enc < (axis2.set_point - 100))){
jebradshaw 0:02d2a7571614 317 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);
jebradshaw 0:02d2a7571614 318 wait(.009);
jebradshaw 0:02d2a7571614 319 }
jebradshaw 0:02d2a7571614 320 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);
jebradshaw 0:02d2a7571614 321 break;
jebradshaw 0:02d2a7571614 322
jebradshaw 0:02d2a7571614 323 case '3':
jebradshaw 0:02d2a7571614 324 pc.printf("Enter value for set point axis 3\r\n");
jebradshaw 0:02d2a7571614 325 pc.scanf("%f",&axis3.set_point);
jebradshaw 0:02d2a7571614 326 pc.printf("%f\r\n",axis3.set_point);
jebradshaw 0:02d2a7571614 327 t.reset();
jebradshaw 0:02d2a7571614 328 while((axis3.enc > (axis3.set_point + 100)) || (axis3.enc < (axis3.set_point - 100))){
jebradshaw 0:02d2a7571614 329 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);
jebradshaw 0:02d2a7571614 330 wait(.009);
jebradshaw 0:02d2a7571614 331 }
jebradshaw 0:02d2a7571614 332 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);
jebradshaw 0:02d2a7571614 333 break;
jebradshaw 0:02d2a7571614 334
jebradshaw 0:02d2a7571614 335 case '4':
jebradshaw 0:02d2a7571614 336 pc.printf("Enter value for set point axis 4\r\n");
jebradshaw 0:02d2a7571614 337 pc.scanf("%f",&axis4.set_point);
jebradshaw 0:02d2a7571614 338 pc.printf("%f\r\n",axis4.set_point);
jebradshaw 0:02d2a7571614 339 t.reset();
jebradshaw 0:02d2a7571614 340 while((axis4.enc > (axis4.set_point + 100)) || (axis4.enc < (axis4.set_point - 100))){
jebradshaw 0:02d2a7571614 341 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);
jebradshaw 0:02d2a7571614 342 wait(.009);
jebradshaw 0:02d2a7571614 343 }
jebradshaw 0:02d2a7571614 344 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);
jebradshaw 0:02d2a7571614 345 break;
jebradshaw 0:02d2a7571614 346
jebradshaw 0:02d2a7571614 347 case '5':
jebradshaw 0:02d2a7571614 348 pc.printf("Enter value for set point axis 5\r\n");
jebradshaw 0:02d2a7571614 349 pc.scanf("%f",&axis5.set_point);
jebradshaw 0:02d2a7571614 350 pc.printf("%f\r\n",axis5.set_point);
jebradshaw 0:02d2a7571614 351 t.reset();
jebradshaw 0:02d2a7571614 352 while((axis5.enc > (axis5.set_point + 100)) || (axis5.enc < (axis5.set_point - 100))){
jebradshaw 0:02d2a7571614 353 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);
jebradshaw 0:02d2a7571614 354 wait(.009);
jebradshaw 0:02d2a7571614 355 }
jebradshaw 0:02d2a7571614 356 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);
jebradshaw 0:02d2a7571614 357 break;
jebradshaw 0:02d2a7571614 358
jebradshaw 0:02d2a7571614 359 case '6':
jebradshaw 0:02d2a7571614 360 pc.printf("Enter value for set point axis 6\r\n");
jebradshaw 0:02d2a7571614 361 pc.scanf("%f",&axis6.set_point);
jebradshaw 0:02d2a7571614 362 pc.printf("%f\r\n",axis6.set_point);
jebradshaw 0:02d2a7571614 363 t.reset();
jebradshaw 0:02d2a7571614 364 while((axis6.enc > (axis6.set_point + 100)) || (axis6.enc < (axis6.set_point - 100))){
jebradshaw 0:02d2a7571614 365 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);
jebradshaw 0:02d2a7571614 366 wait(.009);
jebradshaw 0:02d2a7571614 367 }
jebradshaw 0:02d2a7571614 368 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);
jebradshaw 0:02d2a7571614 369 break;
jebradshaw 0:02d2a7571614 370 }
jebradshaw 0:02d2a7571614 371 }
jebradshaw 0:02d2a7571614 372 if(c == 'P' || c == 'p')
jebradshaw 0:02d2a7571614 373 {
jebradshaw 0:02d2a7571614 374 pc.printf("Enter value for Pk\r\n");
jebradshaw 0:02d2a7571614 375 pc.scanf("%f",&axis1.Pk);
jebradshaw 0:02d2a7571614 376 pc.printf("%f\r\n",axis1.Pk);
jebradshaw 0:02d2a7571614 377 }
jebradshaw 0:02d2a7571614 378 if(c == 'I' || c == 'i')
jebradshaw 0:02d2a7571614 379 {
jebradshaw 0:02d2a7571614 380 pc.printf("Enter value for Ik\r\n");
jebradshaw 0:02d2a7571614 381 pc.scanf("%f",&axis1.Ik);
jebradshaw 0:02d2a7571614 382 pc.printf("%f\r\n",axis1.Ik);
jebradshaw 0:02d2a7571614 383 }
jebradshaw 0:02d2a7571614 384 if(c == 'D' || c == 'd')
jebradshaw 0:02d2a7571614 385 {
jebradshaw 0:02d2a7571614 386 pc.printf("Enter value for Dk\r\n");
jebradshaw 0:02d2a7571614 387 pc.scanf("%f",&axis1.Dk);
jebradshaw 0:02d2a7571614 388 pc.printf("%f\r\n",axis1.Dk);
jebradshaw 0:02d2a7571614 389 }
jebradshaw 0:02d2a7571614 390 if(c == 'Z' || c == 'z')
jebradshaw 0:02d2a7571614 391 {
jebradshaw 0:02d2a7571614 392 pc.printf("Enter axis to zero\r\n");
jebradshaw 0:02d2a7571614 393 pc.scanf("%c",&c);
jebradshaw 0:02d2a7571614 394 switch(c){
jebradshaw 0:02d2a7571614 395 case '1':
jebradshaw 0:02d2a7571614 396 axis1.ls7366->LS7366_reset_counter();
jebradshaw 0:02d2a7571614 397 axis1.ls7366->LS7366_quad_mode_x4();
jebradshaw 0:02d2a7571614 398 axis1.ls7366->LS7366_write_DTR(0);
jebradshaw 0:02d2a7571614 399
jebradshaw 0:02d2a7571614 400 axis1.set_point = 0.0;
jebradshaw 0:02d2a7571614 401 axis1.pid->setSetPoint(0);
jebradshaw 0:02d2a7571614 402 break;
jebradshaw 0:02d2a7571614 403
jebradshaw 0:02d2a7571614 404 case '2':
jebradshaw 0:02d2a7571614 405 axis2.ls7366->LS7366_reset_counter();
jebradshaw 0:02d2a7571614 406 axis2.ls7366->LS7366_quad_mode_x4();
jebradshaw 0:02d2a7571614 407 axis2.ls7366->LS7366_write_DTR(0);
jebradshaw 0:02d2a7571614 408
jebradshaw 0:02d2a7571614 409 axis2.set_point = 0.0;
jebradshaw 0:02d2a7571614 410 axis2.pid->setSetPoint(0);
jebradshaw 0:02d2a7571614 411 break;
jebradshaw 0:02d2a7571614 412
jebradshaw 0:02d2a7571614 413 case '3':
jebradshaw 0:02d2a7571614 414 axis3.ls7366->LS7366_reset_counter();
jebradshaw 0:02d2a7571614 415 axis3.ls7366->LS7366_quad_mode_x4();
jebradshaw 0:02d2a7571614 416 axis3.ls7366->LS7366_write_DTR(0);
jebradshaw 0:02d2a7571614 417
jebradshaw 0:02d2a7571614 418 axis3.set_point = 0.0;
jebradshaw 0:02d2a7571614 419 axis3.pid->setSetPoint(0);
jebradshaw 0:02d2a7571614 420 break;
jebradshaw 0:02d2a7571614 421
jebradshaw 0:02d2a7571614 422 case '4':
jebradshaw 0:02d2a7571614 423 axis4.ls7366->LS7366_reset_counter();
jebradshaw 0:02d2a7571614 424 axis4.ls7366->LS7366_quad_mode_x4();
jebradshaw 0:02d2a7571614 425 axis4.ls7366->LS7366_write_DTR(0);
jebradshaw 0:02d2a7571614 426
jebradshaw 0:02d2a7571614 427 axis4.set_point = 0.0;
jebradshaw 0:02d2a7571614 428 axis4.pid->setSetPoint(0);
jebradshaw 0:02d2a7571614 429 break;
jebradshaw 0:02d2a7571614 430
jebradshaw 0:02d2a7571614 431 case '5':
jebradshaw 0:02d2a7571614 432 axis5.ls7366->LS7366_reset_counter();
jebradshaw 0:02d2a7571614 433 axis5.ls7366->LS7366_quad_mode_x4();
jebradshaw 0:02d2a7571614 434 axis5.ls7366->LS7366_write_DTR(0);
jebradshaw 0:02d2a7571614 435
jebradshaw 0:02d2a7571614 436 axis5.set_point = 0.0;
jebradshaw 0:02d2a7571614 437 axis5.pid->setSetPoint(0);
jebradshaw 0:02d2a7571614 438 break;
jebradshaw 0:02d2a7571614 439
jebradshaw 0:02d2a7571614 440 case '6':
jebradshaw 0:02d2a7571614 441 axis6.ls7366->LS7366_reset_counter();
jebradshaw 0:02d2a7571614 442 axis6.ls7366->LS7366_quad_mode_x4();
jebradshaw 0:02d2a7571614 443 axis6.ls7366->LS7366_write_DTR(0);
jebradshaw 0:02d2a7571614 444
jebradshaw 0:02d2a7571614 445 axis6.set_point = 0.0;
jebradshaw 0:02d2a7571614 446 axis6.pid->setSetPoint(0);
jebradshaw 0:02d2a7571614 447 break;
jebradshaw 0:02d2a7571614 448 }
jebradshaw 0:02d2a7571614 449
jebradshaw 0:02d2a7571614 450 }
jebradshaw 0:02d2a7571614 451 if(c == ' ')
jebradshaw 0:02d2a7571614 452 {
jebradshaw 0:02d2a7571614 453 axis1.ls7366->LS7366_reset_counter();
jebradshaw 0:02d2a7571614 454 axis1.ls7366->LS7366_quad_mode_x4();
jebradshaw 0:02d2a7571614 455 axis1.ls7366->LS7366_write_DTR(0);
jebradshaw 0:02d2a7571614 456
jebradshaw 0:02d2a7571614 457 //read encoder values
jebradshaw 0:02d2a7571614 458 axis1.enc = axis1.ls7366->LS7366_read_counter();
jebradshaw 0:02d2a7571614 459 //long enc2 = LS7366_read_counter(2);
jebradshaw 0:02d2a7571614 460
jebradshaw 0:02d2a7571614 461 //resets the controllers internals
jebradshaw 0:02d2a7571614 462 axis1.pid->reset();
jebradshaw 0:02d2a7571614 463 //start at 0
jebradshaw 0:02d2a7571614 464 axis1.set_point = 0.0;
jebradshaw 0:02d2a7571614 465 axis1.pid->setSetPoint(0);
jebradshaw 0:02d2a7571614 466 }
jebradshaw 0:02d2a7571614 467
jebradshaw 0:02d2a7571614 468 //----- axis1.pid->setTunings(axis1.Pk, axis1.Ik, axis1.Dk);
jebradshaw 0:02d2a7571614 469 //T1_P = 0.0;
jebradshaw 0:02d2a7571614 470 //T1_I = 0.0; //reset integral
jebradshaw 0:02d2a7571614 471 //controller.reset();
jebradshaw 0:02d2a7571614 472 }
jebradshaw 0:02d2a7571614 473
jebradshaw 0:02d2a7571614 474 if((axis1.moveState==0) && (axis2.moveState==0) && (axis3.moveState==0) && (axis4.moveState==0) && (axis5.moveState==0) && (axis6.moveState==0)){
jebradshaw 0:02d2a7571614 475 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,
jebradshaw 0:02d2a7571614 476 axis3.set_point, axis3.pos, axis4.set_point, axis4.pos,axis5.set_point, axis5.pos, axis6.set_point, axis6.pos);
jebradshaw 0:02d2a7571614 477 led2 = 0;
jebradshaw 0:02d2a7571614 478 }
jebradshaw 0:02d2a7571614 479 else
jebradshaw 0:02d2a7571614 480 led2 = 1;
jebradshaw 0:02d2a7571614 481
jebradshaw 0:02d2a7571614 482 wait(.02);
jebradshaw 0:02d2a7571614 483 }//while(1)
jebradshaw 0:02d2a7571614 484 }//main