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:
Wed Sep 30 16:21:14 2015 +0000
Revision:
1:0d7a1f813571
Parent:
0:02d2a7571614
Child:
2:e5d56ee55af6
testing with all 6 axes.

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 1:0d7a1f813571 12 #include "PCF8574.h" //library for the I/O expander (limit switches)
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 1:0d7a1f813571 18 #define SP_TOL 100 // SET POINT TOLERANCE is +/- tolerance for set point command
jebradshaw 0:02d2a7571614 19
jebradshaw 0:02d2a7571614 20 DigitalOut led1(P1_18); //blue
jebradshaw 0:02d2a7571614 21 DigitalOut led2(P1_20); //
jebradshaw 0:02d2a7571614 22 DigitalOut led3(P1_21);
jebradshaw 0:02d2a7571614 23
jebradshaw 0:02d2a7571614 24 Serial pc(P0_2, P0_3); //pc serial interface (USB)
jebradshaw 0:02d2a7571614 25 SPI spi(P0_9, P0_8, P0_7); //MOSI, MISO, SCK
jebradshaw 0:02d2a7571614 26
jebradshaw 0:02d2a7571614 27 int limit0, limit1, limit2, limit3, limit4, limit5; //global limit switch values
jebradshaw 0:02d2a7571614 28
jebradshaw 0:02d2a7571614 29 Timer t;
jebradshaw 1:0d7a1f813571 30 //instantiate axis object NAME(spi bus, LS7366_cs, pwm, dir, analog, limitSwitchAddress, TotalEncoderCounts/axis)
jebradshaw 1:0d7a1f813571 31 Axis axis1(spi, P1_24, P2_5, P1_0, P0_23, &limit0, 25000.0); //base
jebradshaw 1:0d7a1f813571 32 Axis axis2(spi, P1_25, P2_4, P1_1, P0_24, &limit1, 17500); //shoulder
jebradshaw 1:0d7a1f813571 33 Axis axis3(spi, P1_26, P2_3, P1_4, P0_25, &limit2, 20500); //elbow
jebradshaw 1:0d7a1f813571 34 Axis axis4(spi, P1_27, P2_2, P1_8, P0_26, &limit3, 5000); //pitch/roll
jebradshaw 1:0d7a1f813571 35 Axis axis5(spi, P1_28, P2_1, P1_9, P1_30, &limit4, 5000); //pitch/roll
jebradshaw 1:0d7a1f813571 36 Axis axis6(spi, P1_29, P2_0, P1_10, P1_31, &limit5, 5400); //grip
jebradshaw 0:02d2a7571614 37
jebradshaw 0:02d2a7571614 38 PCF8574 pcf(P0_10,P0_11,PCF8574_ADDR,false); // Declare PCF8574 i2c with sda and scl (p28,p27) (10K pullups!)
jebradshaw 0:02d2a7571614 39 //uint8_t data;
jebradshaw 0:02d2a7571614 40 Ticker pulse;
jebradshaw 0:02d2a7571614 41
jebradshaw 0:02d2a7571614 42 static void myerror(std::string msg)
jebradshaw 0:02d2a7571614 43 {
jebradshaw 0:02d2a7571614 44 printf("Error %s\n",msg.c_str());
jebradshaw 0:02d2a7571614 45 exit(1);
jebradshaw 0:02d2a7571614 46 }
jebradshaw 0:02d2a7571614 47
jebradshaw 0:02d2a7571614 48 void updateLimitSwitches(int state){
jebradshaw 0:02d2a7571614 49 if((state & 0x01) == 0x01)
jebradshaw 0:02d2a7571614 50 limit0 = 1;
jebradshaw 0:02d2a7571614 51 else
jebradshaw 0:02d2a7571614 52 limit0 = 0;
jebradshaw 0:02d2a7571614 53
jebradshaw 0:02d2a7571614 54 if((state & 0x02) == 0x02)
jebradshaw 0:02d2a7571614 55 limit1 = 1;
jebradshaw 0:02d2a7571614 56 else
jebradshaw 0:02d2a7571614 57 limit1 = 0;
jebradshaw 0:02d2a7571614 58
jebradshaw 0:02d2a7571614 59 if((state & 0x04) == 0x04)
jebradshaw 0:02d2a7571614 60 limit2 = 1;
jebradshaw 0:02d2a7571614 61 else
jebradshaw 0:02d2a7571614 62 limit2 = 0;
jebradshaw 0:02d2a7571614 63
jebradshaw 0:02d2a7571614 64 if((state & 0x08) == 0x08)
jebradshaw 0:02d2a7571614 65 limit3 = 1;
jebradshaw 0:02d2a7571614 66 else
jebradshaw 0:02d2a7571614 67 limit3 = 0;
jebradshaw 0:02d2a7571614 68
jebradshaw 0:02d2a7571614 69 if((state & 0x10) == 0x10)
jebradshaw 0:02d2a7571614 70 limit4 = 1;
jebradshaw 0:02d2a7571614 71 else
jebradshaw 0:02d2a7571614 72 limit4 = 0;
jebradshaw 0:02d2a7571614 73
jebradshaw 0:02d2a7571614 74 if((state & 0x20) == 0x20)
jebradshaw 0:02d2a7571614 75 limit5 = 1;
jebradshaw 0:02d2a7571614 76 else
jebradshaw 0:02d2a7571614 77 limit5 = 0;
jebradshaw 0:02d2a7571614 78 }
jebradshaw 0:02d2a7571614 79
jebradshaw 0:02d2a7571614 80 void pcf8574_it(uint8_t data, PCF8574 *o)
jebradshaw 0:02d2a7571614 81 {
jebradshaw 0:02d2a7571614 82 int state;
jebradshaw 0:02d2a7571614 83 state = pcf.read();
jebradshaw 0:02d2a7571614 84 printf("PCF8574 interrupt data = %02x\n",state);
jebradshaw 0:02d2a7571614 85 updateLimitSwitches(state);
jebradshaw 0:02d2a7571614 86 }
jebradshaw 0:02d2a7571614 87
jebradshaw 0:02d2a7571614 88 void home_test(void){
jebradshaw 1:0d7a1f813571 89 axis2.pid->setInputLimits(-20000.0, 20000.0);
jebradshaw 1:0d7a1f813571 90 while(*axis2.ptr_limit == 1){
jebradshaw 1:0d7a1f813571 91 axis2.set_point += 100;
jebradshaw 1:0d7a1f813571 92 axis2.pid->setSetPoint(axis2.set_point);
jebradshaw 0:02d2a7571614 93
jebradshaw 1:0d7a1f813571 94 axis3.set_point -= 100;
jebradshaw 1:0d7a1f813571 95 axis3.pid->setSetPoint(axis3.set_point);
jebradshaw 0:02d2a7571614 96
jebradshaw 0:02d2a7571614 97 wait(.05);
jebradshaw 0:02d2a7571614 98 if(axis1.debug)
jebradshaw 0:02d2a7571614 99 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 100 }
jebradshaw 1:0d7a1f813571 101
jebradshaw 1:0d7a1f813571 102 axis2.set_point -= 1000;
jebradshaw 1:0d7a1f813571 103 axis2.pid->setSetPoint(axis2.set_point);
jebradshaw 0:02d2a7571614 104 wait(.5);
jebradshaw 0:02d2a7571614 105
jebradshaw 1:0d7a1f813571 106 axis1.center();
jebradshaw 0:02d2a7571614 107 }
jebradshaw 0:02d2a7571614 108
jebradshaw 0:02d2a7571614 109 void alive(void){
jebradshaw 0:02d2a7571614 110 led1 = !led1;
jebradshaw 0:02d2a7571614 111 if(led1)
jebradshaw 1:0d7a1f813571 112 pulse.attach(&alive, .2); // the address of the function to be attached (flip) and the interval (2 seconds)
jebradshaw 0:02d2a7571614 113 else
jebradshaw 1:0d7a1f813571 114 pulse.attach(&alive, 1.3); // the address of the function to be attached (flip) and the interval (2 seconds)
jebradshaw 1:0d7a1f813571 115 }
jebradshaw 1:0d7a1f813571 116
jebradshaw 1:0d7a1f813571 117 int collisionCheck(void){
jebradshaw 1:0d7a1f813571 118 float current;
jebradshaw 1:0d7a1f813571 119 current = axis1.readCurrent();
jebradshaw 1:0d7a1f813571 120 pc.printf("\r\n%.3f ", current);
jebradshaw 1:0d7a1f813571 121 current = axis2.readCurrent();
jebradshaw 1:0d7a1f813571 122 pc.printf("%.3f ", current);
jebradshaw 1:0d7a1f813571 123 current = axis3.readCurrent();
jebradshaw 1:0d7a1f813571 124 pc.printf("%.3f ", current);
jebradshaw 1:0d7a1f813571 125 current = axis4.readCurrent();
jebradshaw 1:0d7a1f813571 126 pc.printf("%.3f ", current);
jebradshaw 1:0d7a1f813571 127 current = axis5.readCurrent();
jebradshaw 1:0d7a1f813571 128 pc.printf("%.3f ", current);
jebradshaw 1:0d7a1f813571 129 current = axis6.readCurrent();
jebradshaw 1:0d7a1f813571 130 pc.printf("%.3f\r\n", current);
jebradshaw 1:0d7a1f813571 131
jebradshaw 1:0d7a1f813571 132 return 1;
jebradshaw 0:02d2a7571614 133 }
jebradshaw 0:02d2a7571614 134 //------------------- MAIN --------------------------------
jebradshaw 0:02d2a7571614 135 int main()
jebradshaw 0:02d2a7571614 136 {
jebradshaw 0:02d2a7571614 137 wait(.2);
jebradshaw 0:02d2a7571614 138 pulse.attach(&alive, 2.0); // the address of the function to be attached (flip) and the interval (2 seconds)
jebradshaw 0:02d2a7571614 139
jebradshaw 0:02d2a7571614 140 pc.baud(921600);
jebradshaw 0:02d2a7571614 141 //i2c.frequency(100000);
jebradshaw 0:02d2a7571614 142 //LimitSwitch_0.mode(PullUp); //set the mbed to use a pullup resistor
jebradshaw 0:02d2a7571614 143 pc.printf("\r\n%s\r\n", __FILE__); //display the filename (this source file)
jebradshaw 0:02d2a7571614 144
jebradshaw 0:02d2a7571614 145 // Set all IO port bits to 1 to enable inputs and test error
jebradshaw 0:02d2a7571614 146 pcf = 0xff;
jebradshaw 0:02d2a7571614 147 if(pcf.getError() != 0)
jebradshaw 0:02d2a7571614 148 myerror(pcf.getErrorMessage());
jebradshaw 0:02d2a7571614 149
jebradshaw 0:02d2a7571614 150 // Assign interrupt function to pin P0_17 (mbed p12)
jebradshaw 0:02d2a7571614 151 pcf.interrupt(P0_17,&pcf8574_it);
jebradshaw 0:02d2a7571614 152 updateLimitSwitches(pcf.read());
jebradshaw 0:02d2a7571614 153
jebradshaw 0:02d2a7571614 154 if(pcf.getError() != 0)
jebradshaw 0:02d2a7571614 155 myerror(pcf.getErrorMessage());
jebradshaw 0:02d2a7571614 156
jebradshaw 1:0d7a1f813571 157 axis1.init();
jebradshaw 1:0d7a1f813571 158 axis2.init();
jebradshaw 1:0d7a1f813571 159 axis3.init();
jebradshaw 1:0d7a1f813571 160 axis4.init();
jebradshaw 1:0d7a1f813571 161 axis5.init();
jebradshaw 1:0d7a1f813571 162 axis6.init();
jebradshaw 1:0d7a1f813571 163
jebradshaw 0:02d2a7571614 164 axis1.debug = 1;
jebradshaw 0:02d2a7571614 165 axis2.debug = 1;
jebradshaw 1:0d7a1f813571 166 axis3.debug = 1;
jebradshaw 1:0d7a1f813571 167 axis4.debug = 1;
jebradshaw 1:0d7a1f813571 168 axis5.debug = 1;
jebradshaw 1:0d7a1f813571 169 axis6.debug = 1;
jebradshaw 0:02d2a7571614 170
jebradshaw 0:02d2a7571614 171 t.start(); // Set up timer
jebradshaw 0:02d2a7571614 172
jebradshaw 0:02d2a7571614 173 while(1){
jebradshaw 0:02d2a7571614 174
jebradshaw 0:02d2a7571614 175 if(pc.readable())
jebradshaw 0:02d2a7571614 176 {
jebradshaw 0:02d2a7571614 177 char c = pc.getc();
jebradshaw 1:0d7a1f813571 178 if((c == 'A') || (c == 'a')){
jebradshaw 0:02d2a7571614 179 home_test();
jebradshaw 0:02d2a7571614 180 }
jebradshaw 1:0d7a1f813571 181 if(c == '?') //get commands
jebradshaw 0:02d2a7571614 182 {
jebradshaw 0:02d2a7571614 183 pc.printf("T - Trapezoidal Profile Move command\r\n");
jebradshaw 0:02d2a7571614 184 pc.printf("H- Home\r\n");
jebradshaw 0:02d2a7571614 185 pc.printf("S- Set point in encoder counts\r\n");
jebradshaw 1:0d7a1f813571 186 pc.printf("Z - Zero Encoder\r\n");
jebradshaw 1:0d7a1f813571 187 pc.printf("X - Excercise Robotic Arm\r\n");
jebradshaw 1:0d7a1f813571 188 pc.printf("O - Axis to turn On (Default)\r\n");
jebradshaw 1:0d7a1f813571 189 pc.printf("F - Axis to turn Off \r\n");
jebradshaw 1:0d7a1f813571 190 pc.printf("P - Set Proportional Gain on an Axis\r\n");
jebradshaw 1:0d7a1f813571 191 pc.printf("I - Set Integral Gain on an Axis\r\n");
jebradshaw 1:0d7a1f813571 192 pc.printf("D - Set Derivative Gain on an Axis\r\n");
jebradshaw 1:0d7a1f813571 193 pc.printf("spacebar - Emergency Stop\r\n");
jebradshaw 0:02d2a7571614 194
jebradshaw 0:02d2a7571614 195 }
jebradshaw 1:0d7a1f813571 196 if((c == 'X' || c == 'x')) //Excercise all axes
jebradshaw 0:02d2a7571614 197 {
jebradshaw 0:02d2a7571614 198 pc.printf("\r\nPress q to quit Excercise\r\n");
jebradshaw 0:02d2a7571614 199 pc.printf("Received move test command\r\n");
jebradshaw 0:02d2a7571614 200 int qFlag=0;
jebradshaw 0:02d2a7571614 201 float lastmovetime = 0.0;
jebradshaw 0:02d2a7571614 202 t.reset();
jebradshaw 0:02d2a7571614 203 while(qFlag==0){
jebradshaw 0:02d2a7571614 204 float movetime = rand() % 5;
jebradshaw 0:02d2a7571614 205 movetime += 1.0;
jebradshaw 0:02d2a7571614 206 lastmovetime = t.read() + movetime;
jebradshaw 0:02d2a7571614 207
jebradshaw 0:02d2a7571614 208 axis1.moveTrapezoid((rand() % 7000) - 3500, movetime);
jebradshaw 0:02d2a7571614 209 axis2.moveTrapezoid((rand() % 5000) - 2500, movetime);
jebradshaw 0:02d2a7571614 210 axis3.moveTrapezoid((rand() % 3000) - 1500, movetime);
jebradshaw 0:02d2a7571614 211 axis4.moveTrapezoid((rand() % 3000) - 1500, movetime);
jebradshaw 0:02d2a7571614 212 axis5.moveTrapezoid((rand() % 3000) - 1500, movetime);
jebradshaw 0:02d2a7571614 213 axis6.moveTrapezoid((rand() % 3000) - 1500, movetime);
jebradshaw 0:02d2a7571614 214
jebradshaw 0:02d2a7571614 215 while(t.read() <= lastmovetime + 1.0){
jebradshaw 0:02d2a7571614 216 //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 217 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 218 axis3.set_point, axis3.pos, axis4.set_point, axis4.pos,axis5.set_point, axis5.pos, axis6.set_point, axis6.pos);
jebradshaw 0:02d2a7571614 219 wait(.01);
jebradshaw 0:02d2a7571614 220
jebradshaw 0:02d2a7571614 221 if(pc.readable()){ //if user types a 'q' or 'Q'
jebradshaw 0:02d2a7571614 222 char c = pc.getc();
jebradshaw 0:02d2a7571614 223 if(c == 'q' || c == 'Q') //quit after current movement
jebradshaw 0:02d2a7571614 224 qFlag = 1; //set the flag to terminate the excercise
jebradshaw 0:02d2a7571614 225 if(c == ' '){ //stop immediately
jebradshaw 0:02d2a7571614 226 axis1.moveState = 4;
jebradshaw 0:02d2a7571614 227 axis2.moveState = 4;
jebradshaw 0:02d2a7571614 228 axis3.moveState = 4;
jebradshaw 0:02d2a7571614 229 axis4.moveState = 4;
jebradshaw 0:02d2a7571614 230 axis5.moveState = 4;
jebradshaw 0:02d2a7571614 231 axis6.moveState = 4;
jebradshaw 0:02d2a7571614 232 qFlag = 1; //set the flag to terminate the excercise
jebradshaw 0:02d2a7571614 233 break;
jebradshaw 0:02d2a7571614 234 }
jebradshaw 0:02d2a7571614 235 }
jebradshaw 0:02d2a7571614 236 }
jebradshaw 0:02d2a7571614 237 if(t.read() < 0.0){ //if time goes negative, reset the timer
jebradshaw 0:02d2a7571614 238 t.reset();
jebradshaw 0:02d2a7571614 239 }
jebradshaw 0:02d2a7571614 240 }
jebradshaw 0:02d2a7571614 241 }
jebradshaw 1:0d7a1f813571 242 if(c == 'T' || c == 't'){ //move Trapazoid command
jebradshaw 0:02d2a7571614 243 float position = 0.0;
jebradshaw 0:02d2a7571614 244 float time = 0.0;
jebradshaw 0:02d2a7571614 245
jebradshaw 0:02d2a7571614 246 pc.printf("Enter axis to move trapazoid\r\n");
jebradshaw 0:02d2a7571614 247 pc.scanf("%c",&c);
jebradshaw 0:02d2a7571614 248
jebradshaw 0:02d2a7571614 249 pc.printf("\r\n\r\nEnter position:");
jebradshaw 0:02d2a7571614 250 pc.scanf("%f",&position);
jebradshaw 0:02d2a7571614 251 pc.printf("%f\r\n", position);
jebradshaw 0:02d2a7571614 252
jebradshaw 0:02d2a7571614 253 pc.printf("Enter time:");
jebradshaw 0:02d2a7571614 254 pc.scanf("%f",&time);
jebradshaw 0:02d2a7571614 255 pc.printf("%f\r\n", time);
jebradshaw 0:02d2a7571614 256
jebradshaw 0:02d2a7571614 257 switch(c){
jebradshaw 0:02d2a7571614 258 case '1':
jebradshaw 0:02d2a7571614 259 pc.printf("Moving Robotic Axis 1\r\n");
jebradshaw 0:02d2a7571614 260 axis1.moveTrapezoid(position, time);
jebradshaw 0:02d2a7571614 261 break;
jebradshaw 0:02d2a7571614 262
jebradshaw 0:02d2a7571614 263 case '2':
jebradshaw 0:02d2a7571614 264 pc.printf("Moving Robotic Axis 2\r\n");
jebradshaw 0:02d2a7571614 265 axis2.moveTrapezoid(position, time);
jebradshaw 0:02d2a7571614 266 break;
jebradshaw 0:02d2a7571614 267
jebradshaw 0:02d2a7571614 268 case '3':
jebradshaw 0:02d2a7571614 269 pc.printf("Moving Robotic Axis 3\r\n");
jebradshaw 0:02d2a7571614 270 axis3.moveTrapezoid(position, time);
jebradshaw 0:02d2a7571614 271 break;
jebradshaw 0:02d2a7571614 272
jebradshaw 0:02d2a7571614 273 case '4':
jebradshaw 0:02d2a7571614 274 pc.printf("Moving Robotic Axis 4\r\n");
jebradshaw 0:02d2a7571614 275 axis4.moveTrapezoid(position, time);
jebradshaw 0:02d2a7571614 276 break;
jebradshaw 0:02d2a7571614 277
jebradshaw 0:02d2a7571614 278 case '5':
jebradshaw 0:02d2a7571614 279 pc.printf("Moving Robotic Axis 5\r\n");
jebradshaw 0:02d2a7571614 280 axis5.moveTrapezoid(position, time);
jebradshaw 0:02d2a7571614 281 break;
jebradshaw 0:02d2a7571614 282
jebradshaw 0:02d2a7571614 283 case '6':
jebradshaw 0:02d2a7571614 284 pc.printf("Moving Robotic Axis 6\r\n");
jebradshaw 0:02d2a7571614 285 axis6.moveTrapezoid(position, time);
jebradshaw 0:02d2a7571614 286 break;
jebradshaw 0:02d2a7571614 287 }
jebradshaw 0:02d2a7571614 288 }
jebradshaw 1:0d7a1f813571 289 if(c == 'H' || c == 'h'){
jebradshaw 0:02d2a7571614 290 pc.printf("Enter axis to home\r\n");
jebradshaw 0:02d2a7571614 291 pc.scanf("%c",&c);
jebradshaw 0:02d2a7571614 292 switch(c){
jebradshaw 0:02d2a7571614 293 case '1':
jebradshaw 0:02d2a7571614 294 pc.printf("Homing Robotic Axis 1\r\n");
jebradshaw 0:02d2a7571614 295 /* axis2.ls7366->LS7366_reset_counter();
jebradshaw 0:02d2a7571614 296 axis2.ls7366->LS7366_quad_mode_x4();
jebradshaw 0:02d2a7571614 297 axis2.ls7366->LS7366_write_DTR(0);
jebradshaw 0:02d2a7571614 298 axis2.enc = axis1.ls7366->LS7366_read_counter();
jebradshaw 0:02d2a7571614 299 */
jebradshaw 1:0d7a1f813571 300 axis1.center();
jebradshaw 0:02d2a7571614 301 break;
jebradshaw 0:02d2a7571614 302 case '2':
jebradshaw 1:0d7a1f813571 303 pc.printf("Homing Robotic Axis 2\r\n");
jebradshaw 1:0d7a1f813571 304 axis2.center();
jebradshaw 0:02d2a7571614 305 break;
jebradshaw 1:0d7a1f813571 306
jebradshaw 1:0d7a1f813571 307 case '3':
jebradshaw 1:0d7a1f813571 308 pc.printf("Homing Robotic Axis 3\r\n");
jebradshaw 1:0d7a1f813571 309 axis3.center();
jebradshaw 1:0d7a1f813571 310 break;
jebradshaw 1:0d7a1f813571 311
jebradshaw 1:0d7a1f813571 312 case '4':
jebradshaw 1:0d7a1f813571 313 pc.printf("Homing Robotic Axis 4\r\n");
jebradshaw 1:0d7a1f813571 314 axis4.center();
jebradshaw 1:0d7a1f813571 315 break;
jebradshaw 1:0d7a1f813571 316
jebradshaw 1:0d7a1f813571 317 case '5':
jebradshaw 1:0d7a1f813571 318 pc.printf("Homing Robotic Axis 5\r\n");
jebradshaw 1:0d7a1f813571 319 axis5.center();
jebradshaw 1:0d7a1f813571 320 break;
jebradshaw 1:0d7a1f813571 321
jebradshaw 1:0d7a1f813571 322 case '6':
jebradshaw 1:0d7a1f813571 323 pc.printf("Homing Robotic Axis 6\r\n");
jebradshaw 1:0d7a1f813571 324 axis6.center();
jebradshaw 1:0d7a1f813571 325 break;
jebradshaw 0:02d2a7571614 326 }
jebradshaw 0:02d2a7571614 327 }
jebradshaw 1:0d7a1f813571 328 if(c == 'S' || c == 's'){
jebradshaw 0:02d2a7571614 329 pc.printf("Enter axis to give set point\r\n");
jebradshaw 1:0d7a1f813571 330 pc.scanf("%c",&c);
jebradshaw 1:0d7a1f813571 331 pc.printf("Enter value for set point axis %c\r\n", c);
jebradshaw 0:02d2a7571614 332 switch(c){
jebradshaw 1:0d7a1f813571 333 case '1':
jebradshaw 0:02d2a7571614 334 pc.scanf("%f",&axis1.set_point);
jebradshaw 0:02d2a7571614 335 pc.printf("%f\r\n",axis1.set_point);
jebradshaw 0:02d2a7571614 336 t.reset();
jebradshaw 1:0d7a1f813571 337 while((axis1.enc > (axis1.set_point + SP_TOL)) || (axis1.enc < (axis1.set_point - SP_TOL))){
jebradshaw 0:02d2a7571614 338 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 339 wait(.009);
jebradshaw 0:02d2a7571614 340 }
jebradshaw 0:02d2a7571614 341 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 342
jebradshaw 0:02d2a7571614 343 break;
jebradshaw 0:02d2a7571614 344
jebradshaw 0:02d2a7571614 345 case '2':
jebradshaw 0:02d2a7571614 346 pc.scanf("%f",&axis2.set_point);
jebradshaw 0:02d2a7571614 347 pc.printf("%f\r\n",axis2.set_point);
jebradshaw 0:02d2a7571614 348 t.reset();
jebradshaw 1:0d7a1f813571 349 while((axis2.enc > (axis2.set_point + SP_TOL)) || (axis2.enc < (axis2.set_point - SP_TOL))){
jebradshaw 0:02d2a7571614 350 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 351 wait(.009);
jebradshaw 0:02d2a7571614 352 }
jebradshaw 0:02d2a7571614 353 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 354 break;
jebradshaw 0:02d2a7571614 355
jebradshaw 0:02d2a7571614 356 case '3':
jebradshaw 0:02d2a7571614 357 pc.scanf("%f",&axis3.set_point);
jebradshaw 0:02d2a7571614 358 pc.printf("%f\r\n",axis3.set_point);
jebradshaw 0:02d2a7571614 359 t.reset();
jebradshaw 1:0d7a1f813571 360 while((axis3.enc > (axis3.set_point + SP_TOL)) || (axis3.enc < (axis3.set_point - SP_TOL))){
jebradshaw 0:02d2a7571614 361 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 362 wait(.009);
jebradshaw 0:02d2a7571614 363 }
jebradshaw 0:02d2a7571614 364 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 365 break;
jebradshaw 0:02d2a7571614 366
jebradshaw 0:02d2a7571614 367 case '4':
jebradshaw 0:02d2a7571614 368 pc.scanf("%f",&axis4.set_point);
jebradshaw 0:02d2a7571614 369 pc.printf("%f\r\n",axis4.set_point);
jebradshaw 0:02d2a7571614 370 t.reset();
jebradshaw 1:0d7a1f813571 371 while((axis4.enc > (axis4.set_point + SP_TOL)) || (axis4.enc < (axis4.set_point - SP_TOL))){
jebradshaw 0:02d2a7571614 372 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 373 wait(.009);
jebradshaw 0:02d2a7571614 374 }
jebradshaw 0:02d2a7571614 375 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 376 break;
jebradshaw 0:02d2a7571614 377
jebradshaw 0:02d2a7571614 378 case '5':
jebradshaw 0:02d2a7571614 379 pc.scanf("%f",&axis5.set_point);
jebradshaw 0:02d2a7571614 380 pc.printf("%f\r\n",axis5.set_point);
jebradshaw 0:02d2a7571614 381 t.reset();
jebradshaw 1:0d7a1f813571 382 while((axis5.enc > (axis5.set_point + SP_TOL)) || (axis5.enc < (axis5.set_point - 50))){
jebradshaw 0:02d2a7571614 383 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 384 wait(.009);
jebradshaw 0:02d2a7571614 385 }
jebradshaw 0:02d2a7571614 386 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 387 break;
jebradshaw 0:02d2a7571614 388
jebradshaw 0:02d2a7571614 389 case '6':
jebradshaw 0:02d2a7571614 390 pc.scanf("%f",&axis6.set_point);
jebradshaw 0:02d2a7571614 391 pc.printf("%f\r\n",axis6.set_point);
jebradshaw 0:02d2a7571614 392 t.reset();
jebradshaw 1:0d7a1f813571 393 while((axis6.enc > (axis6.set_point + SP_TOL)) || (axis6.enc < (axis6.set_point - SP_TOL))){
jebradshaw 0:02d2a7571614 394 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 395 wait(.009);
jebradshaw 0:02d2a7571614 396 }
jebradshaw 0:02d2a7571614 397 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 398 break;
jebradshaw 0:02d2a7571614 399 }
jebradshaw 0:02d2a7571614 400 }
jebradshaw 1:0d7a1f813571 401 if(c == 'P' || c == 'p'){
jebradshaw 1:0d7a1f813571 402 float temp_Pk;
jebradshaw 1:0d7a1f813571 403 pc.printf("Enter axis to set Pk\r\n");
jebradshaw 1:0d7a1f813571 404 pc.scanf("%c",&c);
jebradshaw 1:0d7a1f813571 405
jebradshaw 1:0d7a1f813571 406 pc.printf("Enter value for Axis%c Pk\r\n", c);
jebradshaw 1:0d7a1f813571 407 pc.scanf("%f",&temp_Pk);
jebradshaw 1:0d7a1f813571 408
jebradshaw 1:0d7a1f813571 409 switch(c){
jebradshaw 1:0d7a1f813571 410 case 1:
jebradshaw 1:0d7a1f813571 411 axis1.Pk = temp_Pk;
jebradshaw 1:0d7a1f813571 412 break;
jebradshaw 1:0d7a1f813571 413
jebradshaw 1:0d7a1f813571 414 case 2:
jebradshaw 1:0d7a1f813571 415 axis2.Pk = temp_Pk;
jebradshaw 1:0d7a1f813571 416 break;
jebradshaw 1:0d7a1f813571 417
jebradshaw 1:0d7a1f813571 418 case 3:
jebradshaw 1:0d7a1f813571 419 axis3.Pk = temp_Pk;
jebradshaw 1:0d7a1f813571 420 break;
jebradshaw 1:0d7a1f813571 421
jebradshaw 1:0d7a1f813571 422 case 4:
jebradshaw 1:0d7a1f813571 423 axis4.Pk = temp_Pk;
jebradshaw 1:0d7a1f813571 424 break;
jebradshaw 1:0d7a1f813571 425
jebradshaw 1:0d7a1f813571 426 case 5:
jebradshaw 1:0d7a1f813571 427 axis5.Pk = temp_Pk;
jebradshaw 1:0d7a1f813571 428 break;
jebradshaw 1:0d7a1f813571 429
jebradshaw 1:0d7a1f813571 430 case 6:
jebradshaw 1:0d7a1f813571 431 axis6.Pk = temp_Pk;
jebradshaw 1:0d7a1f813571 432 break;
jebradshaw 1:0d7a1f813571 433 }
jebradshaw 1:0d7a1f813571 434 pc.printf("Axis%c Pk set to %f\r\n", c, temp_Pk);
jebradshaw 0:02d2a7571614 435 }
jebradshaw 1:0d7a1f813571 436 if(c == 'I' || c == 'i'){
jebradshaw 1:0d7a1f813571 437 float temp_Ik;
jebradshaw 1:0d7a1f813571 438 pc.printf("Enter axis to set Ik\r\n");
jebradshaw 1:0d7a1f813571 439 pc.scanf("%c",&c);
jebradshaw 1:0d7a1f813571 440
jebradshaw 1:0d7a1f813571 441 pc.printf("Enter value for Axis%c Ik\r\n", c);
jebradshaw 1:0d7a1f813571 442 pc.scanf("%f",&temp_Ik);
jebradshaw 1:0d7a1f813571 443
jebradshaw 1:0d7a1f813571 444 switch(c){
jebradshaw 1:0d7a1f813571 445 case 1:
jebradshaw 1:0d7a1f813571 446 axis1.Ik = temp_Ik;
jebradshaw 1:0d7a1f813571 447 break;
jebradshaw 1:0d7a1f813571 448
jebradshaw 1:0d7a1f813571 449 case 2:
jebradshaw 1:0d7a1f813571 450 axis2.Ik = temp_Ik;
jebradshaw 1:0d7a1f813571 451 break;
jebradshaw 1:0d7a1f813571 452
jebradshaw 1:0d7a1f813571 453 case 3:
jebradshaw 1:0d7a1f813571 454 axis3.Ik = temp_Ik;
jebradshaw 1:0d7a1f813571 455 break;
jebradshaw 1:0d7a1f813571 456
jebradshaw 1:0d7a1f813571 457 case 4:
jebradshaw 1:0d7a1f813571 458 axis4.Ik = temp_Ik;
jebradshaw 1:0d7a1f813571 459 break;
jebradshaw 1:0d7a1f813571 460
jebradshaw 1:0d7a1f813571 461 case 5:
jebradshaw 1:0d7a1f813571 462 axis5.Ik = temp_Ik;
jebradshaw 1:0d7a1f813571 463 break;
jebradshaw 1:0d7a1f813571 464
jebradshaw 1:0d7a1f813571 465 case 6:
jebradshaw 1:0d7a1f813571 466 axis6.Ik = temp_Ik;
jebradshaw 1:0d7a1f813571 467 break;
jebradshaw 1:0d7a1f813571 468 }
jebradshaw 1:0d7a1f813571 469 pc.printf("Axis%c Ik set to %f\r\n", c, temp_Ik);
jebradshaw 0:02d2a7571614 470 }
jebradshaw 1:0d7a1f813571 471 if(c == 'D' || c == 'd'){
jebradshaw 1:0d7a1f813571 472 float temp_Dk;
jebradshaw 1:0d7a1f813571 473 pc.printf("Enter axis to set Dk\r\n");
jebradshaw 1:0d7a1f813571 474 pc.scanf("%c",&c);
jebradshaw 1:0d7a1f813571 475
jebradshaw 1:0d7a1f813571 476 pc.printf("Enter value for Axis%c Dk\r\n", c);
jebradshaw 1:0d7a1f813571 477 pc.scanf("%f",&temp_Dk);
jebradshaw 1:0d7a1f813571 478
jebradshaw 1:0d7a1f813571 479 switch(c){
jebradshaw 1:0d7a1f813571 480 case 1:
jebradshaw 1:0d7a1f813571 481 axis1.Dk = temp_Dk;
jebradshaw 1:0d7a1f813571 482 break;
jebradshaw 1:0d7a1f813571 483
jebradshaw 1:0d7a1f813571 484 case 2:
jebradshaw 1:0d7a1f813571 485 axis2.Dk = temp_Dk;
jebradshaw 1:0d7a1f813571 486 break;
jebradshaw 1:0d7a1f813571 487
jebradshaw 1:0d7a1f813571 488 case 3:
jebradshaw 1:0d7a1f813571 489 axis3.Dk = temp_Dk;
jebradshaw 1:0d7a1f813571 490 break;
jebradshaw 1:0d7a1f813571 491
jebradshaw 1:0d7a1f813571 492 case 4:
jebradshaw 1:0d7a1f813571 493 axis4.Dk = temp_Dk;
jebradshaw 1:0d7a1f813571 494 break;
jebradshaw 1:0d7a1f813571 495
jebradshaw 1:0d7a1f813571 496 case 5:
jebradshaw 1:0d7a1f813571 497 axis5.Dk = temp_Dk;
jebradshaw 1:0d7a1f813571 498 break;
jebradshaw 1:0d7a1f813571 499
jebradshaw 1:0d7a1f813571 500 case 6:
jebradshaw 1:0d7a1f813571 501 axis6.Dk = temp_Dk;
jebradshaw 1:0d7a1f813571 502 break;
jebradshaw 1:0d7a1f813571 503 }
jebradshaw 1:0d7a1f813571 504 pc.printf("Axis%c Ik set to %f\r\n", c, temp_Dk);
jebradshaw 0:02d2a7571614 505 }
jebradshaw 1:0d7a1f813571 506 if(c == 'Z' || c == 'z'){
jebradshaw 0:02d2a7571614 507 pc.printf("Enter axis to zero\r\n");
jebradshaw 0:02d2a7571614 508 pc.scanf("%c",&c);
jebradshaw 0:02d2a7571614 509 switch(c){
jebradshaw 0:02d2a7571614 510 case '1':
jebradshaw 0:02d2a7571614 511 axis1.ls7366->LS7366_reset_counter();
jebradshaw 0:02d2a7571614 512 axis1.ls7366->LS7366_quad_mode_x4();
jebradshaw 0:02d2a7571614 513 axis1.ls7366->LS7366_write_DTR(0);
jebradshaw 0:02d2a7571614 514
jebradshaw 0:02d2a7571614 515 axis1.set_point = 0.0;
jebradshaw 0:02d2a7571614 516 axis1.pid->setSetPoint(0);
jebradshaw 0:02d2a7571614 517 break;
jebradshaw 0:02d2a7571614 518
jebradshaw 0:02d2a7571614 519 case '2':
jebradshaw 0:02d2a7571614 520 axis2.ls7366->LS7366_reset_counter();
jebradshaw 0:02d2a7571614 521 axis2.ls7366->LS7366_quad_mode_x4();
jebradshaw 0:02d2a7571614 522 axis2.ls7366->LS7366_write_DTR(0);
jebradshaw 0:02d2a7571614 523
jebradshaw 0:02d2a7571614 524 axis2.set_point = 0.0;
jebradshaw 0:02d2a7571614 525 axis2.pid->setSetPoint(0);
jebradshaw 0:02d2a7571614 526 break;
jebradshaw 0:02d2a7571614 527
jebradshaw 0:02d2a7571614 528 case '3':
jebradshaw 0:02d2a7571614 529 axis3.ls7366->LS7366_reset_counter();
jebradshaw 0:02d2a7571614 530 axis3.ls7366->LS7366_quad_mode_x4();
jebradshaw 0:02d2a7571614 531 axis3.ls7366->LS7366_write_DTR(0);
jebradshaw 0:02d2a7571614 532
jebradshaw 0:02d2a7571614 533 axis3.set_point = 0.0;
jebradshaw 0:02d2a7571614 534 axis3.pid->setSetPoint(0);
jebradshaw 0:02d2a7571614 535 break;
jebradshaw 0:02d2a7571614 536
jebradshaw 0:02d2a7571614 537 case '4':
jebradshaw 0:02d2a7571614 538 axis4.ls7366->LS7366_reset_counter();
jebradshaw 0:02d2a7571614 539 axis4.ls7366->LS7366_quad_mode_x4();
jebradshaw 0:02d2a7571614 540 axis4.ls7366->LS7366_write_DTR(0);
jebradshaw 0:02d2a7571614 541
jebradshaw 0:02d2a7571614 542 axis4.set_point = 0.0;
jebradshaw 0:02d2a7571614 543 axis4.pid->setSetPoint(0);
jebradshaw 0:02d2a7571614 544 break;
jebradshaw 0:02d2a7571614 545
jebradshaw 0:02d2a7571614 546 case '5':
jebradshaw 0:02d2a7571614 547 axis5.ls7366->LS7366_reset_counter();
jebradshaw 0:02d2a7571614 548 axis5.ls7366->LS7366_quad_mode_x4();
jebradshaw 0:02d2a7571614 549 axis5.ls7366->LS7366_write_DTR(0);
jebradshaw 0:02d2a7571614 550
jebradshaw 0:02d2a7571614 551 axis5.set_point = 0.0;
jebradshaw 0:02d2a7571614 552 axis5.pid->setSetPoint(0);
jebradshaw 0:02d2a7571614 553 break;
jebradshaw 0:02d2a7571614 554
jebradshaw 0:02d2a7571614 555 case '6':
jebradshaw 0:02d2a7571614 556 axis6.ls7366->LS7366_reset_counter();
jebradshaw 0:02d2a7571614 557 axis6.ls7366->LS7366_quad_mode_x4();
jebradshaw 0:02d2a7571614 558 axis6.ls7366->LS7366_write_DTR(0);
jebradshaw 0:02d2a7571614 559
jebradshaw 0:02d2a7571614 560 axis6.set_point = 0.0;
jebradshaw 0:02d2a7571614 561 axis6.pid->setSetPoint(0);
jebradshaw 0:02d2a7571614 562 break;
jebradshaw 0:02d2a7571614 563 }
jebradshaw 0:02d2a7571614 564
jebradshaw 0:02d2a7571614 565 }
jebradshaw 1:0d7a1f813571 566 if(c == 'O' || c == 'o'){
jebradshaw 1:0d7a1f813571 567 pc.printf("Enter axis to turn On\r\n");
jebradshaw 1:0d7a1f813571 568 pc.scanf("%c",&c);
jebradshaw 1:0d7a1f813571 569
jebradshaw 1:0d7a1f813571 570 switch(c){
jebradshaw 1:0d7a1f813571 571 case '1':
jebradshaw 1:0d7a1f813571 572 axis1.axisOn();
jebradshaw 1:0d7a1f813571 573 break;
jebradshaw 1:0d7a1f813571 574
jebradshaw 1:0d7a1f813571 575 case '2':
jebradshaw 1:0d7a1f813571 576 axis2.axisOn();
jebradshaw 1:0d7a1f813571 577 break;
jebradshaw 1:0d7a1f813571 578
jebradshaw 1:0d7a1f813571 579 case '3':
jebradshaw 1:0d7a1f813571 580 axis3.axisOn();
jebradshaw 1:0d7a1f813571 581 break;
jebradshaw 1:0d7a1f813571 582
jebradshaw 1:0d7a1f813571 583 case '4':
jebradshaw 1:0d7a1f813571 584 axis4.axisOn();
jebradshaw 1:0d7a1f813571 585 break;
jebradshaw 1:0d7a1f813571 586
jebradshaw 1:0d7a1f813571 587 case '5':
jebradshaw 1:0d7a1f813571 588 axis5.axisOn();
jebradshaw 1:0d7a1f813571 589 break;
jebradshaw 1:0d7a1f813571 590
jebradshaw 1:0d7a1f813571 591 case '6':
jebradshaw 1:0d7a1f813571 592 axis6.axisOn();
jebradshaw 1:0d7a1f813571 593 break;
jebradshaw 1:0d7a1f813571 594 }
jebradshaw 1:0d7a1f813571 595 pc.printf("Axis%d On\r\n", c);
jebradshaw 1:0d7a1f813571 596 }
jebradshaw 1:0d7a1f813571 597 if(c == 'F' || c == 'f'){
jebradshaw 1:0d7a1f813571 598 pc.printf("Enter axis to turn Off\r\n");
jebradshaw 1:0d7a1f813571 599 pc.scanf("%c",&c);
jebradshaw 0:02d2a7571614 600
jebradshaw 1:0d7a1f813571 601 switch(c){
jebradshaw 1:0d7a1f813571 602 case '1':
jebradshaw 1:0d7a1f813571 603 axis1.axisOff();
jebradshaw 1:0d7a1f813571 604 break;
jebradshaw 1:0d7a1f813571 605
jebradshaw 1:0d7a1f813571 606 case '2':
jebradshaw 1:0d7a1f813571 607 axis2.axisOff();
jebradshaw 1:0d7a1f813571 608 break;
jebradshaw 1:0d7a1f813571 609
jebradshaw 1:0d7a1f813571 610 case '3':
jebradshaw 1:0d7a1f813571 611 axis3.axisOff();
jebradshaw 1:0d7a1f813571 612 break;
jebradshaw 1:0d7a1f813571 613
jebradshaw 1:0d7a1f813571 614 case '4':
jebradshaw 1:0d7a1f813571 615 axis4.axisOff();
jebradshaw 1:0d7a1f813571 616 break;
jebradshaw 1:0d7a1f813571 617
jebradshaw 1:0d7a1f813571 618 case '5':
jebradshaw 1:0d7a1f813571 619 axis5.axisOff();
jebradshaw 1:0d7a1f813571 620 break;
jebradshaw 1:0d7a1f813571 621
jebradshaw 1:0d7a1f813571 622 case '6':
jebradshaw 1:0d7a1f813571 623 axis6.axisOff();
jebradshaw 1:0d7a1f813571 624 break;
jebradshaw 1:0d7a1f813571 625 }
jebradshaw 1:0d7a1f813571 626 pc.printf("Axis%d Off\r\n", c);
jebradshaw 0:02d2a7571614 627 }
jebradshaw 1:0d7a1f813571 628 if(c == 'B' || c == 'b'){
jebradshaw 1:0d7a1f813571 629 pc.printf("Enter pitch counts\r\n");
jebradshaw 1:0d7a1f813571 630
jebradshaw 1:0d7a1f813571 631 float counts;
jebradshaw 1:0d7a1f813571 632
jebradshaw 1:0d7a1f813571 633 pc.scanf("%f",counts);
jebradshaw 1:0d7a1f813571 634 axis4.set_point = counts;
jebradshaw 1:0d7a1f813571 635 axis5.set_point = -counts;
jebradshaw 1:0d7a1f813571 636
jebradshaw 1:0d7a1f813571 637 pc.printf("%f\r\n",&counts);
jebradshaw 1:0d7a1f813571 638 t.reset();
jebradshaw 1:0d7a1f813571 639 while(axis4.enc > (axis4.set_point + SP_TOL) ||
jebradshaw 1:0d7a1f813571 640 axis4.enc < (axis4.set_point - SP_TOL) &&
jebradshaw 1:0d7a1f813571 641 axis5.enc > (axis5.set_point + SP_TOL) ||
jebradshaw 1:0d7a1f813571 642 axis5.enc < (axis5.set_point - SP_TOL)){
jebradshaw 1:0d7a1f813571 643 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);
jebradshaw 1:0d7a1f813571 644 wait(.009);
jebradshaw 1:0d7a1f813571 645 }
jebradshaw 1:0d7a1f813571 646 }
jebradshaw 1:0d7a1f813571 647 if(c == 'N' || c == 'n'){
jebradshaw 1:0d7a1f813571 648 pc.printf("Enter rotate counts\r\n");
jebradshaw 1:0d7a1f813571 649
jebradshaw 1:0d7a1f813571 650 float counts;
jebradshaw 1:0d7a1f813571 651
jebradshaw 1:0d7a1f813571 652 pc.scanf("%f",&counts);
jebradshaw 1:0d7a1f813571 653 axis4.set_point = counts;
jebradshaw 1:0d7a1f813571 654 axis5.set_point = counts;
jebradshaw 1:0d7a1f813571 655
jebradshaw 1:0d7a1f813571 656 pc.printf("%f\r\n",counts);
jebradshaw 1:0d7a1f813571 657 t.reset();
jebradshaw 1:0d7a1f813571 658 while(axis4.enc > (axis4.set_point + SP_TOL) ||
jebradshaw 1:0d7a1f813571 659 axis4.enc < (axis4.set_point - SP_TOL) &&
jebradshaw 1:0d7a1f813571 660 axis5.enc > (axis5.set_point + SP_TOL) ||
jebradshaw 1:0d7a1f813571 661 axis5.enc < (axis5.set_point - SP_TOL)){
jebradshaw 1:0d7a1f813571 662 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);
jebradshaw 1:0d7a1f813571 663 wait(.009);
jebradshaw 1:0d7a1f813571 664 }
jebradshaw 1:0d7a1f813571 665 }
jebradshaw 1:0d7a1f813571 666 }//command was received
jebradshaw 0:02d2a7571614 667
jebradshaw 0:02d2a7571614 668 if((axis1.moveState==0) && (axis2.moveState==0) && (axis3.moveState==0) && (axis4.moveState==0) && (axis5.moveState==0) && (axis6.moveState==0)){
jebradshaw 0:02d2a7571614 669 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 670 axis3.set_point, axis3.pos, axis4.set_point, axis4.pos,axis5.set_point, axis5.pos, axis6.set_point, axis6.pos);
jebradshaw 0:02d2a7571614 671 led2 = 0;
jebradshaw 0:02d2a7571614 672 }
jebradshaw 0:02d2a7571614 673 else
jebradshaw 0:02d2a7571614 674 led2 = 1;
jebradshaw 0:02d2a7571614 675
jebradshaw 0:02d2a7571614 676 wait(.02);
jebradshaw 0:02d2a7571614 677 }//while(1)
jebradshaw 0:02d2a7571614 678 }//main
jebradshaw 1:0d7a1f813571 679