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:
Mon Dec 28 19:59:24 2015 +0000
Revision:
4:890c104546e9
Parent:
3:1892943e3f25
20151228 - Removed PCF8574 I/O Expander and used direct external interrupt inputs for limit/homing switches

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 4:890c104546e9 8 // 20151218 - Eliminated PCF8574 I2C I/O expander and attached limit switches to P0_22 - P0_17
jebradshaw 4:890c104546e9 9 // pins as external interrupts.
jebradshaw 0:02d2a7571614 10
jebradshaw 0:02d2a7571614 11 #include "mbed.h"
jebradshaw 0:02d2a7571614 12 #include "Axis.h"
jebradshaw 0:02d2a7571614 13 #include "stdlib.h"
jebradshaw 0:02d2a7571614 14
jebradshaw 0:02d2a7571614 15 #include <string>
jebradshaw 0:02d2a7571614 16
jebradshaw 0:02d2a7571614 17 #define PI 3.14159
jebradshaw 1:0d7a1f813571 18 #define SP_TOL 100 // SET POINT TOLERANCE is +/- tolerance for set point command
jebradshaw 0:02d2a7571614 19
jebradshaw 4:890c104546e9 20 // Assign interrupt function to pin P0_17 (mbed p12)
jebradshaw 4:890c104546e9 21
jebradshaw 0:02d2a7571614 22 DigitalOut led1(P1_18); //blue
jebradshaw 0:02d2a7571614 23 DigitalOut led2(P1_20); //
jebradshaw 0:02d2a7571614 24 DigitalOut led3(P1_21);
jebradshaw 0:02d2a7571614 25
jebradshaw 0:02d2a7571614 26 Serial pc(P0_2, P0_3); //pc serial interface (USB)
jebradshaw 0:02d2a7571614 27 SPI spi(P0_9, P0_8, P0_7); //MOSI, MISO, SCK
jebradshaw 0:02d2a7571614 28
jebradshaw 4:890c104546e9 29 DigitalIn limit1_pin(P0_22);
jebradshaw 4:890c104546e9 30 DigitalIn limit2_pin(P0_21);
jebradshaw 4:890c104546e9 31 DigitalIn limit3_pin(P0_20);
jebradshaw 4:890c104546e9 32 DigitalIn limit4_pin(P0_19);
jebradshaw 4:890c104546e9 33 DigitalIn limit5_pin(P0_18);
jebradshaw 4:890c104546e9 34 DigitalIn limit6_pin(P0_17);
jebradshaw 4:890c104546e9 35
jebradshaw 4:890c104546e9 36 InterruptIn limit1_int(P0_22);
jebradshaw 4:890c104546e9 37 InterruptIn limit2_int(P0_21);
jebradshaw 4:890c104546e9 38 InterruptIn limit3_int(P0_20);
jebradshaw 4:890c104546e9 39 InterruptIn limit4_int(P0_19);
jebradshaw 4:890c104546e9 40 InterruptIn limit5_int(P0_18);
jebradshaw 4:890c104546e9 41 InterruptIn limit6_int(P0_17);
jebradshaw 4:890c104546e9 42
jebradshaw 3:1892943e3f25 43 int limit1, limit2, limit3, limit4, limit5, limit6; //global limit switch values
jebradshaw 3:1892943e3f25 44 float axis1_I,axis2_I,axis3_I,axis4_I,axis5_I,axis6_I;
jebradshaw 3:1892943e3f25 45 int streamFlag=0;
jebradshaw 0:02d2a7571614 46 Timer t;
jebradshaw 1:0d7a1f813571 47 //instantiate axis object NAME(spi bus, LS7366_cs, pwm, dir, analog, limitSwitchAddress, TotalEncoderCounts/axis)
jebradshaw 3:1892943e3f25 48 Axis axis1(spi, P1_24, P2_5, P1_0, P0_23, &limit1, 25000.0); //base
jebradshaw 3:1892943e3f25 49 Axis axis2(spi, P1_25, P2_4, P1_1, P0_24, &limit2, 17500); //shoulder
jebradshaw 3:1892943e3f25 50 Axis axis3(spi, P1_26, P2_3, P1_4, P0_25, &limit3, 20500); //elbow
jebradshaw 3:1892943e3f25 51 Axis axis4(spi, P1_27, P2_2, P1_8, P0_26, &limit4, 5000); //pitch/roll
jebradshaw 3:1892943e3f25 52 Axis axis5(spi, P1_28, P2_1, P1_9, P1_30, &limit5, 5000); //pitch/roll
jebradshaw 3:1892943e3f25 53 Axis axis6(spi, P1_29, P2_0, P1_10, P1_31, &limit6, 5400); //grip
jebradshaw 0:02d2a7571614 54
jebradshaw 0:02d2a7571614 55 Ticker pulse;
jebradshaw 3:1892943e3f25 56 Ticker colCheck;
jebradshaw 0:02d2a7571614 57
jebradshaw 0:02d2a7571614 58
jebradshaw 3:1892943e3f25 59 void zero_axis(int axis){
jebradshaw 3:1892943e3f25 60 switch(axis){
jebradshaw 3:1892943e3f25 61 case 1:
jebradshaw 3:1892943e3f25 62 axis1.zero();
jebradshaw 3:1892943e3f25 63 break;
jebradshaw 3:1892943e3f25 64
jebradshaw 3:1892943e3f25 65 case 2:
jebradshaw 3:1892943e3f25 66 axis2.zero();
jebradshaw 3:1892943e3f25 67 break;
jebradshaw 3:1892943e3f25 68
jebradshaw 3:1892943e3f25 69 case 3:
jebradshaw 3:1892943e3f25 70 axis3.zero();
jebradshaw 3:1892943e3f25 71 break;
jebradshaw 3:1892943e3f25 72
jebradshaw 3:1892943e3f25 73 case 4:
jebradshaw 3:1892943e3f25 74 axis4.zero();
jebradshaw 3:1892943e3f25 75 break;
jebradshaw 3:1892943e3f25 76
jebradshaw 3:1892943e3f25 77 case 5:
jebradshaw 3:1892943e3f25 78 axis5.zero();
jebradshaw 3:1892943e3f25 79 break;
jebradshaw 3:1892943e3f25 80
jebradshaw 3:1892943e3f25 81 case 6:
jebradshaw 3:1892943e3f25 82 axis6.zero();
jebradshaw 3:1892943e3f25 83 break;
jebradshaw 3:1892943e3f25 84 }
jebradshaw 0:02d2a7571614 85 }
jebradshaw 0:02d2a7571614 86
jebradshaw 3:1892943e3f25 87 void zero_all(void){
jebradshaw 3:1892943e3f25 88 for(int i=1;i<=6;i++){
jebradshaw 3:1892943e3f25 89 zero_axis(i);
jebradshaw 3:1892943e3f25 90 wait(.005);
jebradshaw 3:1892943e3f25 91 }
jebradshaw 3:1892943e3f25 92 }
jebradshaw 3:1892943e3f25 93
jebradshaw 0:02d2a7571614 94 void alive(void){
jebradshaw 0:02d2a7571614 95 led1 = !led1;
jebradshaw 0:02d2a7571614 96 if(led1)
jebradshaw 1:0d7a1f813571 97 pulse.attach(&alive, .2); // the address of the function to be attached (flip) and the interval (2 seconds)
jebradshaw 0:02d2a7571614 98 else
jebradshaw 1:0d7a1f813571 99 pulse.attach(&alive, 1.3); // the address of the function to be attached (flip) and the interval (2 seconds)
jebradshaw 1:0d7a1f813571 100 }
jebradshaw 1:0d7a1f813571 101
jebradshaw 3:1892943e3f25 102 void collisionCheck(void){
jebradshaw 3:1892943e3f25 103 float stall_I = .3;
jebradshaw 3:1892943e3f25 104
jebradshaw 3:1892943e3f25 105 axis1_I = axis1.readCurrent();
jebradshaw 3:1892943e3f25 106 axis2_I = axis2.readCurrent();
jebradshaw 3:1892943e3f25 107 axis3_I = axis3.readCurrent();
jebradshaw 3:1892943e3f25 108 axis4_I = axis4.readCurrent();
jebradshaw 3:1892943e3f25 109 axis5_I = axis5.readCurrent();
jebradshaw 3:1892943e3f25 110 axis6_I = axis6.readCurrent();
jebradshaw 3:1892943e3f25 111 // pc.printf("%.3f %.3f %.3f %.3f %.3f %.3f \r\n", axis1_I, axis2_I, axis3_I, axis4_I, axis5_I, axis6_I);
jebradshaw 3:1892943e3f25 112
jebradshaw 3:1892943e3f25 113 //analog channels 1 and 2 are damaged on initial prototype test bed
jebradshaw 3:1892943e3f25 114 if(axis1_I > stall_I){
jebradshaw 3:1892943e3f25 115 pc.printf("Axis 1 collision detected");
jebradshaw 3:1892943e3f25 116 axis1.axisOff();
jebradshaw 3:1892943e3f25 117 axis2.axisOff();
jebradshaw 3:1892943e3f25 118 axis3.axisOff();
jebradshaw 3:1892943e3f25 119 axis4.axisOff();
jebradshaw 3:1892943e3f25 120 axis5.axisOff();
jebradshaw 3:1892943e3f25 121 axis6.axisOff();
jebradshaw 3:1892943e3f25 122 }
jebradshaw 3:1892943e3f25 123 if(axis2_I > stall_I){
jebradshaw 3:1892943e3f25 124 pc.printf("Axis 2 collision detected");
jebradshaw 3:1892943e3f25 125 axis1.axisOff();
jebradshaw 3:1892943e3f25 126 axis2.axisOff();
jebradshaw 3:1892943e3f25 127 axis3.axisOff();
jebradshaw 3:1892943e3f25 128 axis4.axisOff();
jebradshaw 3:1892943e3f25 129 axis5.axisOff();
jebradshaw 3:1892943e3f25 130 axis6.axisOff();
jebradshaw 3:1892943e3f25 131 }
jebradshaw 3:1892943e3f25 132 if(axis3_I > stall_I){
jebradshaw 3:1892943e3f25 133 pc.printf("Axis 3 collision detected");
jebradshaw 3:1892943e3f25 134 axis1.axisOff();
jebradshaw 3:1892943e3f25 135 axis2.axisOff();
jebradshaw 3:1892943e3f25 136 axis3.axisOff();
jebradshaw 3:1892943e3f25 137 axis4.axisOff();
jebradshaw 3:1892943e3f25 138 axis5.axisOff();
jebradshaw 3:1892943e3f25 139 axis6.axisOff();
jebradshaw 3:1892943e3f25 140 }
jebradshaw 3:1892943e3f25 141 if(axis4_I > stall_I){
jebradshaw 3:1892943e3f25 142 pc.printf("Axis 4 collision detected");
jebradshaw 3:1892943e3f25 143 axis1.axisOff();
jebradshaw 3:1892943e3f25 144 axis2.axisOff();
jebradshaw 3:1892943e3f25 145 axis3.axisOff();
jebradshaw 3:1892943e3f25 146 axis4.axisOff();
jebradshaw 3:1892943e3f25 147 axis5.axisOff();
jebradshaw 3:1892943e3f25 148 axis6.axisOff();
jebradshaw 3:1892943e3f25 149 }
jebradshaw 3:1892943e3f25 150 if(axis5_I > stall_I){
jebradshaw 3:1892943e3f25 151 pc.printf("Axis 5 collision detected");
jebradshaw 3:1892943e3f25 152 axis1.axisOff();
jebradshaw 3:1892943e3f25 153 axis2.axisOff();
jebradshaw 3:1892943e3f25 154 axis3.axisOff();
jebradshaw 3:1892943e3f25 155 axis4.axisOff();
jebradshaw 3:1892943e3f25 156 axis5.axisOff();
jebradshaw 3:1892943e3f25 157 axis6.axisOff();
jebradshaw 3:1892943e3f25 158 }
jebradshaw 3:1892943e3f25 159 if(axis6_I > stall_I){
jebradshaw 3:1892943e3f25 160 pc.printf("Axis 6 collision detected");
jebradshaw 3:1892943e3f25 161 axis1.axisOff();
jebradshaw 3:1892943e3f25 162 axis2.axisOff();
jebradshaw 3:1892943e3f25 163 axis3.axisOff();
jebradshaw 3:1892943e3f25 164 axis4.axisOff();
jebradshaw 3:1892943e3f25 165 axis5.axisOff();
jebradshaw 3:1892943e3f25 166 axis6.axisOff();
jebradshaw 3:1892943e3f25 167 }
jebradshaw 3:1892943e3f25 168 }
jebradshaw 3:1892943e3f25 169
jebradshaw 3:1892943e3f25 170 void home_test(void){
jebradshaw 3:1892943e3f25 171
jebradshaw 3:1892943e3f25 172 axis1.zero();
jebradshaw 3:1892943e3f25 173 axis2.zero();
jebradshaw 3:1892943e3f25 174 axis3.zero();
jebradshaw 3:1892943e3f25 175
jebradshaw 3:1892943e3f25 176 axis1.pid->setInputLimits(-30000, 30000);
jebradshaw 3:1892943e3f25 177 axis2.pid->setInputLimits(-30000, 30000);
jebradshaw 3:1892943e3f25 178 axis3.pid->setInputLimits(-30000, 30000);
jebradshaw 3:1892943e3f25 179
jebradshaw 3:1892943e3f25 180 for(float delta=0.0;delta<200.0 && (*axis2.ptr_limit == 1) && (*axis3.ptr_limit == 1);delta+=100){
jebradshaw 3:1892943e3f25 181 axis3.set_point = delta;
jebradshaw 3:1892943e3f25 182 axis4.set_point = .23 * delta;
jebradshaw 3:1892943e3f25 183 axis5.set_point = .23 * -delta;
jebradshaw 3:1892943e3f25 184 wait(.5);
jebradshaw 3:1892943e3f25 185 }
jebradshaw 3:1892943e3f25 186 zero_axis(3);
jebradshaw 3:1892943e3f25 187
jebradshaw 3:1892943e3f25 188 for(float delta=0.0;delta>-13000.0 && (*axis3.ptr_limit == 1);delta-=100){
jebradshaw 3:1892943e3f25 189 axis3.set_point = delta;
jebradshaw 3:1892943e3f25 190 axis4.set_point = .249 * delta;
jebradshaw 3:1892943e3f25 191 axis5.set_point = .249 * -delta;
jebradshaw 3:1892943e3f25 192 wait(.5);
jebradshaw 3:1892943e3f25 193 }
jebradshaw 3:1892943e3f25 194
jebradshaw 3:1892943e3f25 195 for(float delta=0.0;delta>-18000.0 && (*axis2.ptr_limit == 1);delta-=100){
jebradshaw 3:1892943e3f25 196 axis2.set_point = delta;
jebradshaw 3:1892943e3f25 197 axis3.set_point = -delta;
jebradshaw 3:1892943e3f25 198 axis4.set_point = .249 * delta;
jebradshaw 3:1892943e3f25 199 axis5.set_point += .249 * -delta;
jebradshaw 3:1892943e3f25 200 wait(.5);
jebradshaw 3:1892943e3f25 201 }
jebradshaw 3:1892943e3f25 202 zero_axis(2);
jebradshaw 3:1892943e3f25 203
jebradshaw 3:1892943e3f25 204 for(float delta=0.0;delta<300.0 ;delta-=10){
jebradshaw 3:1892943e3f25 205 axis3.set_point = delta;
jebradshaw 3:1892943e3f25 206 axis4.set_point = .249 *-delta;
jebradshaw 3:1892943e3f25 207 axis5.set_point = .249 * delta;
jebradshaw 3:1892943e3f25 208 wait(.1);
jebradshaw 3:1892943e3f25 209 }
jebradshaw 3:1892943e3f25 210 zero_axis(2);
jebradshaw 3:1892943e3f25 211 }
jebradshaw 3:1892943e3f25 212
jebradshaw 3:1892943e3f25 213 int home_pitch_wrist(void){
jebradshaw 3:1892943e3f25 214 axis4.pid->setInputLimits(-50000, 50000);
jebradshaw 3:1892943e3f25 215 axis5.pid->setInputLimits(-50000, 50000);
jebradshaw 3:1892943e3f25 216
jebradshaw 3:1892943e3f25 217 axis4.axisOn();
jebradshaw 3:1892943e3f25 218 axis5.axisOn();
jebradshaw 3:1892943e3f25 219
jebradshaw 3:1892943e3f25 220 axis4.zero();
jebradshaw 3:1892943e3f25 221 axis5.zero();
jebradshaw 1:0d7a1f813571 222
jebradshaw 3:1892943e3f25 223 //first test to see if limit switch is already pressed
jebradshaw 3:1892943e3f25 224 if(limit4 == 0){
jebradshaw 3:1892943e3f25 225 pc.printf("Stage 1\r\n");
jebradshaw 3:1892943e3f25 226 pc.printf("Limit switch 4 is already closed, moving up to release switch\r\n");
jebradshaw 3:1892943e3f25 227 //move wrist up until limit switch is released again
jebradshaw 3:1892943e3f25 228 while(limit4 == 0){
jebradshaw 3:1892943e3f25 229 pc.printf("axis4=%.1f axis5=%.1f sw=%1d I4=%f I5=%f \r\n", axis4.pos, axis5.pos, limit4, axis4.readCurrent(), axis5.readCurrent());
jebradshaw 3:1892943e3f25 230 axis4.set_point -= 10;
jebradshaw 3:1892943e3f25 231 axis5.set_point += 10;
jebradshaw 3:1892943e3f25 232 wait(.08);
jebradshaw 3:1892943e3f25 233 }
jebradshaw 3:1892943e3f25 234 pc.printf("Switched released\r\n");
jebradshaw 3:1892943e3f25 235 axis4.zero();
jebradshaw 3:1892943e3f25 236 axis5.zero();
jebradshaw 3:1892943e3f25 237 pc.printf("Encoders zeroed\r\n");
jebradshaw 3:1892943e3f25 238 wait(.02);
jebradshaw 3:1892943e3f25 239
jebradshaw 3:1892943e3f25 240 pc.printf("Moving up to zero\r\n");
jebradshaw 3:1892943e3f25 241 axis4.set_point = -1350;
jebradshaw 3:1892943e3f25 242 axis5.set_point = 1350;
jebradshaw 3:1892943e3f25 243
jebradshaw 3:1892943e3f25 244 wait(2.0);
jebradshaw 3:1892943e3f25 245
jebradshaw 3:1892943e3f25 246 return 0; //successful home of gripper
jebradshaw 3:1892943e3f25 247 }
jebradshaw 3:1892943e3f25 248 else{
jebradshaw 3:1892943e3f25 249 pc.printf("Stage 2\r\n");
jebradshaw 3:1892943e3f25 250 axis4.zero(); //zero wrist motors
jebradshaw 3:1892943e3f25 251 axis5.zero();
jebradshaw 3:1892943e3f25 252 pc.printf("Move down\r\n");
jebradshaw 3:1892943e3f25 253 while(limit4 == 1){
jebradshaw 3:1892943e3f25 254 pc.printf("axis4=%.1f axis5=%.1f sw=%1d I4=%f I5=%f \r\n", axis4.pos, axis5.pos, limit4, axis4.readCurrent(), axis5.readCurrent());
jebradshaw 3:1892943e3f25 255 axis4.set_point += 50;
jebradshaw 3:1892943e3f25 256 axis5.set_point -= 50;
jebradshaw 3:1892943e3f25 257 wait(.05);
jebradshaw 3:1892943e3f25 258 if(axis4.readCurrent() > .25){
jebradshaw 3:1892943e3f25 259 pc.printf("Over Current detected on Axis 3\r\n");
jebradshaw 3:1892943e3f25 260 axis4.zero();
jebradshaw 3:1892943e3f25 261 axis5.zero();
jebradshaw 3:1892943e3f25 262
jebradshaw 3:1892943e3f25 263 axis4.set_point -= 3500;
jebradshaw 3:1892943e3f25 264 axis5.set_point += 3500;
jebradshaw 3:1892943e3f25 265
jebradshaw 3:1892943e3f25 266 wait(2.0);
jebradshaw 3:1892943e3f25 267
jebradshaw 3:1892943e3f25 268 axis4.zero();
jebradshaw 3:1892943e3f25 269 axis5.zero();
jebradshaw 3:1892943e3f25 270
jebradshaw 3:1892943e3f25 271 return -1;
jebradshaw 3:1892943e3f25 272 }
jebradshaw 3:1892943e3f25 273 if(axis5.readCurrent() > .25){
jebradshaw 3:1892943e3f25 274 pc.printf("Over Current detected on Axis 5\r\n");
jebradshaw 3:1892943e3f25 275 axis4.zero();
jebradshaw 3:1892943e3f25 276 axis5.zero();
jebradshaw 3:1892943e3f25 277
jebradshaw 3:1892943e3f25 278 axis4.set_point -= 3500;
jebradshaw 3:1892943e3f25 279 axis5.set_point += 3500;
jebradshaw 3:1892943e3f25 280
jebradshaw 3:1892943e3f25 281 wait(2.0);
jebradshaw 3:1892943e3f25 282
jebradshaw 3:1892943e3f25 283 axis4.zero();
jebradshaw 3:1892943e3f25 284 axis5.zero();
jebradshaw 3:1892943e3f25 285
jebradshaw 3:1892943e3f25 286 return -2;
jebradshaw 3:1892943e3f25 287 }
jebradshaw 3:1892943e3f25 288 }
jebradshaw 3:1892943e3f25 289
jebradshaw 3:1892943e3f25 290 if(limit4 == 0){
jebradshaw 3:1892943e3f25 291 while(limit4 == 0){
jebradshaw 3:1892943e3f25 292 pc.printf("axis4=%.1f axis5=%.1f sw=%1d I4=%f I5=%f \r\n", axis4.pos, axis5.pos, limit4, axis4.readCurrent(), axis5.readCurrent());
jebradshaw 3:1892943e3f25 293 axis4.set_point -= 50;
jebradshaw 3:1892943e3f25 294 axis5.set_point += 50;
jebradshaw 3:1892943e3f25 295 wait(.08);
jebradshaw 3:1892943e3f25 296 if(axis4.readCurrent() > .25){
jebradshaw 3:1892943e3f25 297 pc.printf("Over Current detected on Axis 4\r\n");
jebradshaw 3:1892943e3f25 298 axis4.zero();
jebradshaw 3:1892943e3f25 299 axis5.zero();
jebradshaw 3:1892943e3f25 300
jebradshaw 3:1892943e3f25 301 axis4.set_point -= 3500;
jebradshaw 3:1892943e3f25 302 axis5.set_point += 3500;
jebradshaw 3:1892943e3f25 303
jebradshaw 3:1892943e3f25 304 wait(2.0);
jebradshaw 3:1892943e3f25 305
jebradshaw 3:1892943e3f25 306 axis4.zero();
jebradshaw 3:1892943e3f25 307 axis5.zero();
jebradshaw 3:1892943e3f25 308
jebradshaw 3:1892943e3f25 309 return -1;
jebradshaw 3:1892943e3f25 310 }
jebradshaw 3:1892943e3f25 311 if(axis5.readCurrent() > .25){
jebradshaw 3:1892943e3f25 312 pc.printf("Over Current detected on Axis 5\r\n");
jebradshaw 3:1892943e3f25 313 axis4.zero();
jebradshaw 3:1892943e3f25 314 axis5.zero();
jebradshaw 3:1892943e3f25 315
jebradshaw 3:1892943e3f25 316 axis4.set_point -= 3500;
jebradshaw 3:1892943e3f25 317 axis5.set_point += 3500;
jebradshaw 3:1892943e3f25 318
jebradshaw 3:1892943e3f25 319 wait(2.0);
jebradshaw 3:1892943e3f25 320
jebradshaw 3:1892943e3f25 321 axis4.zero();
jebradshaw 3:1892943e3f25 322 axis5.zero();
jebradshaw 3:1892943e3f25 323
jebradshaw 3:1892943e3f25 324 return -2;
jebradshaw 3:1892943e3f25 325 }
jebradshaw 3:1892943e3f25 326 }
jebradshaw 3:1892943e3f25 327 axis4.zero();
jebradshaw 3:1892943e3f25 328 axis5.zero();
jebradshaw 3:1892943e3f25 329 wait(.02);
jebradshaw 3:1892943e3f25 330
jebradshaw 3:1892943e3f25 331 axis4.set_point = -1350;
jebradshaw 3:1892943e3f25 332 axis5.set_point = 1350;
jebradshaw 3:1892943e3f25 333
jebradshaw 3:1892943e3f25 334 wait(1.2);
jebradshaw 3:1892943e3f25 335 axis4.zero();
jebradshaw 3:1892943e3f25 336 axis5.zero();
jebradshaw 3:1892943e3f25 337
jebradshaw 3:1892943e3f25 338 return 0; //successful home of gripper
jebradshaw 3:1892943e3f25 339 }
jebradshaw 3:1892943e3f25 340 }
jebradshaw 3:1892943e3f25 341 return -3; //should have homed by now
jebradshaw 3:1892943e3f25 342 }
jebradshaw 3:1892943e3f25 343
jebradshaw 3:1892943e3f25 344 void home_rotate_wrist(void){
jebradshaw 3:1892943e3f25 345 axis4.axisOn();
jebradshaw 3:1892943e3f25 346 axis5.axisOn();
jebradshaw 3:1892943e3f25 347
jebradshaw 3:1892943e3f25 348 while(limit5 == 0){
jebradshaw 3:1892943e3f25 349 axis4.set_point -= 100;
jebradshaw 3:1892943e3f25 350 axis5.set_point -= 100;
jebradshaw 3:1892943e3f25 351 wait(.05);
jebradshaw 3:1892943e3f25 352 pc.printf("axis4=%.1f axis5=%.1f sw=%1d I4=%f I5=%f \r\n", axis4.pos, axis5.pos, limit5, axis4.readCurrent(), axis5.readCurrent());
jebradshaw 3:1892943e3f25 353 }
jebradshaw 3:1892943e3f25 354 axis4.set_point -= 10;
jebradshaw 3:1892943e3f25 355 axis5.set_point -= 10;
jebradshaw 3:1892943e3f25 356 wait(.05);
jebradshaw 3:1892943e3f25 357
jebradshaw 3:1892943e3f25 358 while(limit5 == 1){
jebradshaw 3:1892943e3f25 359 pc.printf("axis4=%.1f axis5=%.1f sw=%1d I4=%f I5=%f \r\n", axis4.pos, axis5.pos, limit5, axis4.readCurrent(), axis5.readCurrent());
jebradshaw 3:1892943e3f25 360 axis4.set_point += 10;
jebradshaw 3:1892943e3f25 361 axis5.set_point += 10;
jebradshaw 3:1892943e3f25 362 wait(.03);
jebradshaw 3:1892943e3f25 363 }
jebradshaw 3:1892943e3f25 364
jebradshaw 3:1892943e3f25 365 axis4.zero();
jebradshaw 3:1892943e3f25 366 axis5.zero();
jebradshaw 3:1892943e3f25 367
jebradshaw 3:1892943e3f25 368 pc.printf("Find amount to rotate to after switch is high again...\r\n");
jebradshaw 3:1892943e3f25 369 wait(1.0);
jebradshaw 3:1892943e3f25 370
jebradshaw 3:1892943e3f25 371 for(float pos=0;pos>-800.0;pos-=100){
jebradshaw 3:1892943e3f25 372 axis4.set_point = pos;
jebradshaw 3:1892943e3f25 373 axis5.set_point = pos;
jebradshaw 3:1892943e3f25 374 wait(.05);
jebradshaw 3:1892943e3f25 375 pc.printf("axis4=%.1f axis5=%.1f sw=%1d I4=%f I5=%f \r\n", axis4.pos, axis5.pos, limit5, axis4.readCurrent(), axis5.readCurrent());
jebradshaw 3:1892943e3f25 376 }
jebradshaw 3:1892943e3f25 377 axis4.zero();
jebradshaw 3:1892943e3f25 378 axis5.zero();
jebradshaw 3:1892943e3f25 379 }
jebradshaw 3:1892943e3f25 380
jebradshaw 3:1892943e3f25 381 void cal_gripper(void){
jebradshaw 3:1892943e3f25 382 axis6.axisOn();
jebradshaw 3:1892943e3f25 383 pc.printf("\r\n Axis On, Homeing Gripper\r\n");
jebradshaw 3:1892943e3f25 384 pc.printf("axis6=%.1f I6=%f \r\n", axis6.pos, axis6.readCurrent());
jebradshaw 3:1892943e3f25 385 axis6.zero();
jebradshaw 3:1892943e3f25 386
jebradshaw 3:1892943e3f25 387 while((axis6.readCurrent() < .8) && (axis6.set_point > -6000)){
jebradshaw 3:1892943e3f25 388 axis6.set_point -= 10;
jebradshaw 3:1892943e3f25 389 wait(.01);
jebradshaw 3:1892943e3f25 390 pc.printf("axis6=%.1f I6=%f \r\n", axis6.pos, axis6.readCurrent());
jebradshaw 3:1892943e3f25 391 }
jebradshaw 3:1892943e3f25 392 axis6.set_point += 50;
jebradshaw 3:1892943e3f25 393 wait(.05);
jebradshaw 3:1892943e3f25 394 axis6.zero();
jebradshaw 3:1892943e3f25 395 wait(.02);
jebradshaw 3:1892943e3f25 396
jebradshaw 3:1892943e3f25 397 while((axis6.readCurrent() < .8) && (axis6.set_point < 6000)){
jebradshaw 3:1892943e3f25 398 axis6.set_point += 10;
jebradshaw 3:1892943e3f25 399 wait(.01);
jebradshaw 3:1892943e3f25 400 pc.printf("axis6=%.1f I6=%f \r\n", axis6.pos, axis6.readCurrent());
jebradshaw 3:1892943e3f25 401 }
jebradshaw 3:1892943e3f25 402
jebradshaw 3:1892943e3f25 403 axis6.totalCounts = axis6.set_point;
jebradshaw 3:1892943e3f25 404 wait(.05);
jebradshaw 3:1892943e3f25 405
jebradshaw 3:1892943e3f25 406 axis6.set_point = axis6.totalCounts/2.0; //~4500 total span, ~2200 to half close
jebradshaw 3:1892943e3f25 407 wait(2.0);
jebradshaw 3:1892943e3f25 408 // axis6.zero(); //may remove later for 0 = grip closed
jebradshaw 3:1892943e3f25 409 }
jebradshaw 3:1892943e3f25 410
jebradshaw 3:1892943e3f25 411 void all_on(void){
jebradshaw 3:1892943e3f25 412 axis1.axisOn();
jebradshaw 3:1892943e3f25 413 axis2.axisOn();
jebradshaw 3:1892943e3f25 414 axis3.axisOn();
jebradshaw 3:1892943e3f25 415 axis4.axisOn();
jebradshaw 3:1892943e3f25 416 axis5.axisOn();
jebradshaw 3:1892943e3f25 417 axis6.axisOn();
jebradshaw 3:1892943e3f25 418 }
jebradshaw 3:1892943e3f25 419
jebradshaw 3:1892943e3f25 420 void all_off(void){
jebradshaw 3:1892943e3f25 421 axis1.axisOff();
jebradshaw 3:1892943e3f25 422 axis2.axisOff();
jebradshaw 3:1892943e3f25 423 axis3.axisOff();
jebradshaw 3:1892943e3f25 424 axis4.axisOff();
jebradshaw 3:1892943e3f25 425 axis5.axisOff();
jebradshaw 3:1892943e3f25 426 axis6.axisOff();
jebradshaw 0:02d2a7571614 427 }
jebradshaw 4:890c104546e9 428
jebradshaw 4:890c104546e9 429 void limit1_irq(void){
jebradshaw 4:890c104546e9 430 limit1 = limit1_pin;
jebradshaw 4:890c104546e9 431
jebradshaw 4:890c104546e9 432 if(limit1)
jebradshaw 4:890c104546e9 433 limit1_int.fall(&limit1_irq);
jebradshaw 4:890c104546e9 434 else
jebradshaw 4:890c104546e9 435 limit1_int.rise(&limit1_irq);
jebradshaw 4:890c104546e9 436 }
jebradshaw 4:890c104546e9 437
jebradshaw 4:890c104546e9 438 void limit2_irq(void){
jebradshaw 4:890c104546e9 439 limit2 = limit2_pin;
jebradshaw 4:890c104546e9 440
jebradshaw 4:890c104546e9 441 if(limit2)
jebradshaw 4:890c104546e9 442 limit2_int.fall(&limit2_irq);
jebradshaw 4:890c104546e9 443 else
jebradshaw 4:890c104546e9 444 limit2_int.rise(&limit2_irq);
jebradshaw 4:890c104546e9 445 }
jebradshaw 4:890c104546e9 446
jebradshaw 4:890c104546e9 447 void limit3_irq(void){
jebradshaw 4:890c104546e9 448 limit3 = limit3_pin;
jebradshaw 4:890c104546e9 449
jebradshaw 4:890c104546e9 450 if(limit3)
jebradshaw 4:890c104546e9 451 limit3_int.fall(&limit3_irq);
jebradshaw 4:890c104546e9 452 else
jebradshaw 4:890c104546e9 453 limit3_int.rise(&limit3_irq);
jebradshaw 4:890c104546e9 454 }
jebradshaw 4:890c104546e9 455
jebradshaw 4:890c104546e9 456 void limit4_irq(void){
jebradshaw 4:890c104546e9 457 limit4 = limit4_pin;
jebradshaw 4:890c104546e9 458
jebradshaw 4:890c104546e9 459 if(limit4)
jebradshaw 4:890c104546e9 460 limit4_int.fall(&limit4_irq);
jebradshaw 4:890c104546e9 461 else
jebradshaw 4:890c104546e9 462 limit4_int.rise(&limit4_irq);
jebradshaw 4:890c104546e9 463 }
jebradshaw 4:890c104546e9 464
jebradshaw 4:890c104546e9 465 void limit5_irq(void){
jebradshaw 4:890c104546e9 466 limit5 = limit5_pin;
jebradshaw 4:890c104546e9 467
jebradshaw 4:890c104546e9 468 if(limit5)
jebradshaw 4:890c104546e9 469 limit5_int.fall(&limit5_irq);
jebradshaw 4:890c104546e9 470 else
jebradshaw 4:890c104546e9 471 limit5_int.rise(&limit5_irq);
jebradshaw 4:890c104546e9 472 }
jebradshaw 4:890c104546e9 473
jebradshaw 4:890c104546e9 474 void limit6_irq(void){
jebradshaw 4:890c104546e9 475 limit6 = limit6_pin;
jebradshaw 4:890c104546e9 476
jebradshaw 4:890c104546e9 477 if(limit6)
jebradshaw 4:890c104546e9 478 limit6_int.fall(&limit6_irq);
jebradshaw 4:890c104546e9 479 else
jebradshaw 4:890c104546e9 480 limit6_int.rise(&limit6_irq);
jebradshaw 4:890c104546e9 481 }
jebradshaw 4:890c104546e9 482 void init_limitSwitches(void){
jebradshaw 4:890c104546e9 483
jebradshaw 4:890c104546e9 484 //Limit switch 1 initial state
jebradshaw 4:890c104546e9 485 limit1 = limit1_pin;
jebradshaw 4:890c104546e9 486 if(limit1)
jebradshaw 4:890c104546e9 487 limit1_int.fall(&limit1_irq);
jebradshaw 4:890c104546e9 488 else
jebradshaw 4:890c104546e9 489 limit1_int.rise(&limit1_irq);
jebradshaw 4:890c104546e9 490
jebradshaw 4:890c104546e9 491 //Limit switch 2 initial state
jebradshaw 4:890c104546e9 492 limit2 = limit2_pin;
jebradshaw 4:890c104546e9 493 if(limit2)
jebradshaw 4:890c104546e9 494 limit2_int.fall(&limit2_irq);
jebradshaw 4:890c104546e9 495 else
jebradshaw 4:890c104546e9 496 limit2_int.rise(&limit2_irq);
jebradshaw 4:890c104546e9 497
jebradshaw 4:890c104546e9 498 //Limit switch 3 initial state
jebradshaw 4:890c104546e9 499 limit3 = limit3_pin;
jebradshaw 4:890c104546e9 500 if(limit3)
jebradshaw 4:890c104546e9 501 limit3_int.fall(&limit3_irq);
jebradshaw 4:890c104546e9 502 else
jebradshaw 4:890c104546e9 503 limit3_int.rise(&limit3_irq);
jebradshaw 4:890c104546e9 504
jebradshaw 4:890c104546e9 505 //Limit switch 4 initial state
jebradshaw 4:890c104546e9 506 limit4 = limit4_pin;
jebradshaw 4:890c104546e9 507 if(limit4)
jebradshaw 4:890c104546e9 508 limit4_int.fall(&limit4_irq);
jebradshaw 4:890c104546e9 509 else
jebradshaw 4:890c104546e9 510 limit4_int.rise(&limit4_irq);
jebradshaw 4:890c104546e9 511
jebradshaw 4:890c104546e9 512 //Limit switch 5 initial state
jebradshaw 4:890c104546e9 513 limit5 = limit5_pin;
jebradshaw 4:890c104546e9 514 if(limit5)
jebradshaw 4:890c104546e9 515 limit5_int.fall(&limit5_irq);
jebradshaw 4:890c104546e9 516 else
jebradshaw 4:890c104546e9 517 limit5_int.rise(&limit5_irq);
jebradshaw 4:890c104546e9 518
jebradshaw 4:890c104546e9 519 //Limit switch 6 initial state
jebradshaw 4:890c104546e9 520 limit6 = limit6_pin;
jebradshaw 4:890c104546e9 521 if(limit6)
jebradshaw 4:890c104546e9 522 limit6_int.fall(&limit6_irq);
jebradshaw 4:890c104546e9 523 else
jebradshaw 4:890c104546e9 524 limit6_int.rise(&limit6_irq);
jebradshaw 4:890c104546e9 525
jebradshaw 4:890c104546e9 526 }
jebradshaw 0:02d2a7571614 527 //------------------- MAIN --------------------------------
jebradshaw 0:02d2a7571614 528 int main()
jebradshaw 0:02d2a7571614 529 {
jebradshaw 3:1892943e3f25 530 wait(.5);
jebradshaw 0:02d2a7571614 531 pulse.attach(&alive, 2.0); // the address of the function to be attached (flip) and the interval (2 seconds)
jebradshaw 0:02d2a7571614 532
jebradshaw 0:02d2a7571614 533 pc.baud(921600);
jebradshaw 0:02d2a7571614 534 pc.printf("\r\n%s\r\n", __FILE__); //display the filename (this source file)
jebradshaw 0:02d2a7571614 535
jebradshaw 4:890c104546e9 536 init_limitSwitches(); //get initial states of limit switches
jebradshaw 0:02d2a7571614 537
jebradshaw 1:0d7a1f813571 538 axis1.init();
jebradshaw 1:0d7a1f813571 539 axis2.init();
jebradshaw 1:0d7a1f813571 540 axis3.init();
jebradshaw 1:0d7a1f813571 541 axis4.init();
jebradshaw 1:0d7a1f813571 542 axis5.init();
jebradshaw 1:0d7a1f813571 543 axis6.init();
jebradshaw 1:0d7a1f813571 544
jebradshaw 4:890c104546e9 545 axis6.Pk = 40.0;
jebradshaw 4:890c104546e9 546 axis6.Ik = 20.0;
jebradshaw 3:1892943e3f25 547
jebradshaw 3:1892943e3f25 548 // axis1.debug = 1;
jebradshaw 3:1892943e3f25 549 // axis2.debug = 1;
jebradshaw 3:1892943e3f25 550 // axis3.debug = 1;
jebradshaw 3:1892943e3f25 551 // axis4.debug = 1;
jebradshaw 3:1892943e3f25 552 // axis5.debug = 1;
jebradshaw 3:1892943e3f25 553 // axis6.debug = 1;
jebradshaw 0:02d2a7571614 554
jebradshaw 0:02d2a7571614 555 t.start(); // Set up timer
jebradshaw 0:02d2a7571614 556
jebradshaw 0:02d2a7571614 557 while(1){
jebradshaw 0:02d2a7571614 558
jebradshaw 0:02d2a7571614 559 if(pc.readable())
jebradshaw 0:02d2a7571614 560 {
jebradshaw 0:02d2a7571614 561 char c = pc.getc();
jebradshaw 3:1892943e3f25 562
jebradshaw 1:0d7a1f813571 563 if(c == '?') //get commands
jebradshaw 0:02d2a7571614 564 {
jebradshaw 3:1892943e3f25 565 pc.printf("? - This menu of commands\r\n");
jebradshaw 3:1892943e3f25 566 pc.printf("0 - Zero all encoder channels\r\n");
jebradshaw 3:1892943e3f25 567 pc.printf("A - All: Enable/Disable All axes. Then 'O' for On and 'F' for Off\r\n");
jebradshaw 3:1892943e3f25 568 pc.printf("C - Current: Stream the Motor Currents\r\n");
jebradshaw 3:1892943e3f25 569 pc.printf("J - Stream Flag: Enable/Disable Stream. Then '1' for On and '0' for Off\r\n");
jebradshaw 3:1892943e3f25 570 pc.printf("W - Wrist: Home the Gripper Wrist\r\n");
jebradshaw 3:1892943e3f25 571 pc.printf("U - Up: Bring up from typical starting position (Not HOME!)\r\n");
jebradshaw 0:02d2a7571614 572 pc.printf("T - Trapezoidal Profile Move command\r\n");
jebradshaw 0:02d2a7571614 573 pc.printf("H- Home\r\n");
jebradshaw 0:02d2a7571614 574 pc.printf("S- Set point in encoder counts\r\n");
jebradshaw 3:1892943e3f25 575 pc.printf("Z - Zero Encoder channel\r\n");
jebradshaw 1:0d7a1f813571 576 pc.printf("X - Excercise Robotic Arm\r\n");
jebradshaw 3:1892943e3f25 577 pc.printf("O - Axis to turn On \r\n");
jebradshaw 3:1892943e3f25 578 pc.printf("F - Axis to turn Off (Default)\r\n");
jebradshaw 3:1892943e3f25 579 pc.printf("\r\nP - Set Proportional Gain on an Axis\r\n");
jebradshaw 1:0d7a1f813571 580 pc.printf("I - Set Integral Gain on an Axis\r\n");
jebradshaw 3:1892943e3f25 581 pc.printf("D - Set Derivative Gain on an Axis\r\n");
jebradshaw 3:1892943e3f25 582 pc.printf("\r\nB - Pitch Gripper\r\n");
jebradshaw 3:1892943e3f25 583 pc.printf("N - Rotate Gripper\r\n");
jebradshaw 3:1892943e3f25 584 pc.printf("G - Home Gripper\r\n");
jebradshaw 1:0d7a1f813571 585 pc.printf("spacebar - Emergency Stop\r\n");
jebradshaw 3:1892943e3f25 586
jebradshaw 3:1892943e3f25 587 pc.printf("Press any key to continue.\r\n");
jebradshaw 0:02d2a7571614 588
jebradshaw 3:1892943e3f25 589 pc.scanf("%c",&c);
jebradshaw 3:1892943e3f25 590 c = '\0'; //re-zero c
jebradshaw 3:1892943e3f25 591 }
jebradshaw 3:1892943e3f25 592
jebradshaw 3:1892943e3f25 593 if(c == '0'){ //zero all encoders and channels
jebradshaw 3:1892943e3f25 594 zero_all();
jebradshaw 3:1892943e3f25 595 }
jebradshaw 3:1892943e3f25 596 // All: Enable/Disable ALL motors (On or Off)
jebradshaw 3:1892943e3f25 597 if((c == 'A') || (c == 'a')){
jebradshaw 3:1892943e3f25 598 pc.printf("All: 'o' for all On, 'f' for all off\r\n");
jebradshaw 3:1892943e3f25 599
jebradshaw 3:1892943e3f25 600 pc.scanf("%c",&c);
jebradshaw 3:1892943e3f25 601 if((c == 'O' || c == 'o')){
jebradshaw 3:1892943e3f25 602 all_on();
jebradshaw 3:1892943e3f25 603 }
jebradshaw 3:1892943e3f25 604 if((c == 'F' || c == 'f')){
jebradshaw 3:1892943e3f25 605 all_off();
jebradshaw 3:1892943e3f25 606 }
jebradshaw 3:1892943e3f25 607 c = '\0'; //clear c
jebradshaw 3:1892943e3f25 608 }
jebradshaw 3:1892943e3f25 609
jebradshaw 3:1892943e3f25 610 //Temporary command for Wrist Pitch
jebradshaw 3:1892943e3f25 611 if(c == 'B' || c == 'b'){
jebradshaw 3:1892943e3f25 612 pc.printf("Enter wrist pitch counts\r\n");
jebradshaw 3:1892943e3f25 613
jebradshaw 3:1892943e3f25 614 float counts;
jebradshaw 3:1892943e3f25 615
jebradshaw 3:1892943e3f25 616 pc.scanf("%f",&counts);
jebradshaw 3:1892943e3f25 617 axis4.set_point += counts;
jebradshaw 3:1892943e3f25 618 axis5.set_point += -counts;
jebradshaw 3:1892943e3f25 619
jebradshaw 3:1892943e3f25 620 pc.printf("%f\r\n",counts);
jebradshaw 3:1892943e3f25 621 t.reset();
jebradshaw 3:1892943e3f25 622 while((axis4.pos > (axis4.set_point + SP_TOL) ||
jebradshaw 3:1892943e3f25 623 axis4.pos < (axis4.set_point - SP_TOL)) &&
jebradshaw 3:1892943e3f25 624 (axis5.pos > (axis5.set_point + SP_TOL) ||
jebradshaw 3:1892943e3f25 625 axis5.pos < (axis5.set_point - SP_TOL))){
jebradshaw 3:1892943e3f25 626 pc.printf("T=%.2f SP4=%.3f pos4=%.3f SP5=%.3f pos5=%.3f \r\n", t.read(), axis4.set_point, axis4.pos, axis5.set_point, axis5.pos);
jebradshaw 3:1892943e3f25 627 wait(.009);
jebradshaw 3:1892943e3f25 628 }
jebradshaw 0:02d2a7571614 629 }
jebradshaw 3:1892943e3f25 630
jebradshaw 3:1892943e3f25 631 //wrist rotate
jebradshaw 3:1892943e3f25 632 if(c == 'N' || c == 'n'){
jebradshaw 3:1892943e3f25 633 pc.printf("Enter wrist rotate counts\r\n");
jebradshaw 3:1892943e3f25 634
jebradshaw 3:1892943e3f25 635 float counts;
jebradshaw 3:1892943e3f25 636
jebradshaw 3:1892943e3f25 637 pc.scanf("%f",&counts);
jebradshaw 3:1892943e3f25 638 axis4.set_point += counts;
jebradshaw 3:1892943e3f25 639 axis5.set_point += counts;
jebradshaw 3:1892943e3f25 640
jebradshaw 3:1892943e3f25 641 pc.printf("%f\r\n",counts);
jebradshaw 3:1892943e3f25 642 t.reset();
jebradshaw 3:1892943e3f25 643 while((axis4.pos > (axis4.set_point + SP_TOL) ||
jebradshaw 3:1892943e3f25 644 axis4.pos < (axis4.set_point - SP_TOL)) &&
jebradshaw 3:1892943e3f25 645 (axis5.pos > (axis5.set_point + SP_TOL) ||
jebradshaw 3:1892943e3f25 646 axis5.pos < (axis5.set_point - SP_TOL))){
jebradshaw 3:1892943e3f25 647 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 3:1892943e3f25 648 wait(.009);
jebradshaw 3:1892943e3f25 649 }
jebradshaw 3:1892943e3f25 650 }
jebradshaw 3:1892943e3f25 651
jebradshaw 3:1892943e3f25 652 //Current Measurement: Stream the motor currents
jebradshaw 3:1892943e3f25 653 if((c == 'C') || (c == 'c')){
jebradshaw 3:1892943e3f25 654 int analogFlag = 0;
jebradshaw 3:1892943e3f25 655 while(analogFlag == 0){
jebradshaw 3:1892943e3f25 656 axis1.readCurrent();
jebradshaw 3:1892943e3f25 657 axis2.readCurrent();
jebradshaw 3:1892943e3f25 658 axis3.readCurrent();
jebradshaw 3:1892943e3f25 659 axis4.readCurrent();
jebradshaw 3:1892943e3f25 660 axis5.readCurrent();
jebradshaw 3:1892943e3f25 661 axis6.readCurrent();
jebradshaw 3:1892943e3f25 662 pc.printf("%.3f %.3f %.3f %.3f %.3f %.3f \r\n", axis1.motCurrent, axis2.motCurrent, axis3.motCurrent, axis4.motCurrent, axis5.motCurrent, axis6.motCurrent);
jebradshaw 3:1892943e3f25 663 wait(.02);
jebradshaw 3:1892943e3f25 664
jebradshaw 3:1892943e3f25 665 if(pc.readable()){ //if user types a 'q' or 'Q'
jebradshaw 3:1892943e3f25 666 char c = pc.getc();
jebradshaw 3:1892943e3f25 667 if(c == 'q' || c == 'Q') //quit after current movement
jebradshaw 3:1892943e3f25 668 analogFlag = 1;
jebradshaw 3:1892943e3f25 669 }
jebradshaw 3:1892943e3f25 670 }
jebradshaw 3:1892943e3f25 671 }
jebradshaw 3:1892943e3f25 672
jebradshaw 3:1892943e3f25 673 //Limit: Limit Switch Monitor
jebradshaw 3:1892943e3f25 674 if((c == 'L') || (c == 'l')){
jebradshaw 3:1892943e3f25 675 int limitFlag = 1;
jebradshaw 3:1892943e3f25 676
jebradshaw 3:1892943e3f25 677 while(limitFlag){
jebradshaw 3:1892943e3f25 678 pc.printf("%1d %1d %1d %1d %1d %1d %1d\r\n", limit1, limit2, limit3, limit4, limit5, limit6);
jebradshaw 3:1892943e3f25 679 wait(.02);
jebradshaw 3:1892943e3f25 680
jebradshaw 3:1892943e3f25 681 if(pc.readable()){ //if user types a 'q' or 'Q'
jebradshaw 3:1892943e3f25 682 char c = pc.getc();
jebradshaw 3:1892943e3f25 683 if(c == 'q' || c == 'Q') //quit after current movement
jebradshaw 3:1892943e3f25 684 limitFlag = 0;;
jebradshaw 3:1892943e3f25 685 }
jebradshaw 3:1892943e3f25 686 }
jebradshaw 3:1892943e3f25 687 }
jebradshaw 3:1892943e3f25 688
jebradshaw 3:1892943e3f25 689 //W: Wrist home fuction
jebradshaw 3:1892943e3f25 690 if((c == 'W') || (c == 'w')){
jebradshaw 3:1892943e3f25 691 home_pitch_wrist();
jebradshaw 3:1892943e3f25 692 home_rotate_wrist();
jebradshaw 3:1892943e3f25 693 }
jebradshaw 3:1892943e3f25 694
jebradshaw 3:1892943e3f25 695 //gripper home
jebradshaw 3:1892943e3f25 696 if((c == 'G') || (c == 'g')){
jebradshaw 3:1892943e3f25 697 cal_gripper();
jebradshaw 3:1892943e3f25 698 }
jebradshaw 3:1892943e3f25 699
jebradshaw 3:1892943e3f25 700 //Up: Bring up from typical starting position
jebradshaw 3:1892943e3f25 701 if((c == 'U') || (c == 'u')){
jebradshaw 3:1892943e3f25 702 pc.printf("Bring up from typical unpowered position\r\n");
jebradshaw 3:1892943e3f25 703
jebradshaw 3:1892943e3f25 704 axis1.zero();
jebradshaw 3:1892943e3f25 705 axis2.zero();
jebradshaw 3:1892943e3f25 706 axis3.zero();
jebradshaw 3:1892943e3f25 707 axis4.zero();
jebradshaw 3:1892943e3f25 708 axis5.zero();
jebradshaw 3:1892943e3f25 709 axis6.zero();
jebradshaw 3:1892943e3f25 710
jebradshaw 3:1892943e3f25 711 all_on();
jebradshaw 3:1892943e3f25 712 axis2.set_point += 8000;
jebradshaw 3:1892943e3f25 713 wait(3.5);
jebradshaw 3:1892943e3f25 714 axis2.zero();
jebradshaw 3:1892943e3f25 715 axis3.set_point -= 4500;
jebradshaw 3:1892943e3f25 716 wait(2.5);
jebradshaw 3:1892943e3f25 717 axis3.zero();
jebradshaw 3:1892943e3f25 718
jebradshaw 3:1892943e3f25 719 home_pitch_wrist();
jebradshaw 3:1892943e3f25 720 home_rotate_wrist();
jebradshaw 3:1892943e3f25 721 cal_gripper();
jebradshaw 3:1892943e3f25 722 }
jebradshaw 3:1892943e3f25 723
jebradshaw 3:1892943e3f25 724 //Exercise: Randomly exercise all axes
jebradshaw 3:1892943e3f25 725 if((c == 'X' || c == 'x')) //Exercise all axes
jebradshaw 0:02d2a7571614 726 {
jebradshaw 3:1892943e3f25 727 pc.printf("\r\nPress q to quit Exercise\r\n");
jebradshaw 0:02d2a7571614 728 pc.printf("Received move test command\r\n");
jebradshaw 0:02d2a7571614 729 int qFlag=0;
jebradshaw 0:02d2a7571614 730 float lastmovetime = 0.0;
jebradshaw 0:02d2a7571614 731 while(qFlag==0){
jebradshaw 3:1892943e3f25 732 t.reset();
jebradshaw 3:1892943e3f25 733 float movetime = rand() % 7;
jebradshaw 0:02d2a7571614 734 movetime += 1.0;
jebradshaw 0:02d2a7571614 735 lastmovetime = t.read() + movetime;
jebradshaw 0:02d2a7571614 736
jebradshaw 3:1892943e3f25 737 axis1.moveTrapezoid((rand() % 2000) - 1000, movetime);
jebradshaw 3:1892943e3f25 738 wait(.05);
jebradshaw 0:02d2a7571614 739 axis2.moveTrapezoid((rand() % 5000) - 2500, movetime);
jebradshaw 3:1892943e3f25 740 wait(.05);
jebradshaw 3:1892943e3f25 741 axis3.moveTrapezoid((rand() % 5000) - 2500, movetime);
jebradshaw 3:1892943e3f25 742 wait(.05);
jebradshaw 0:02d2a7571614 743 axis4.moveTrapezoid((rand() % 3000) - 1500, movetime);
jebradshaw 3:1892943e3f25 744 wait(.05);
jebradshaw 0:02d2a7571614 745 axis5.moveTrapezoid((rand() % 3000) - 1500, movetime);
jebradshaw 3:1892943e3f25 746 wait(.05);
jebradshaw 3:1892943e3f25 747 axis6.moveTrapezoid((rand() % 3000), movetime);
jebradshaw 3:1892943e3f25 748 wait(.05);
jebradshaw 0:02d2a7571614 749
jebradshaw 0:02d2a7571614 750 while(t.read() <= lastmovetime + 1.0){
jebradshaw 0:02d2a7571614 751 //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 752 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 753 axis3.set_point, axis3.pos, axis4.set_point, axis4.pos,axis5.set_point, axis5.pos, axis6.set_point, axis6.pos);
jebradshaw 0:02d2a7571614 754 wait(.01);
jebradshaw 0:02d2a7571614 755
jebradshaw 0:02d2a7571614 756 if(pc.readable()){ //if user types a 'q' or 'Q'
jebradshaw 0:02d2a7571614 757 char c = pc.getc();
jebradshaw 0:02d2a7571614 758 if(c == 'q' || c == 'Q') //quit after current movement
jebradshaw 0:02d2a7571614 759 qFlag = 1; //set the flag to terminate the excercise
jebradshaw 0:02d2a7571614 760 if(c == ' '){ //stop immediately
jebradshaw 0:02d2a7571614 761 axis1.moveState = 4;
jebradshaw 0:02d2a7571614 762 axis2.moveState = 4;
jebradshaw 0:02d2a7571614 763 axis3.moveState = 4;
jebradshaw 0:02d2a7571614 764 axis4.moveState = 4;
jebradshaw 0:02d2a7571614 765 axis5.moveState = 4;
jebradshaw 0:02d2a7571614 766 axis6.moveState = 4;
jebradshaw 0:02d2a7571614 767 qFlag = 1; //set the flag to terminate the excercise
jebradshaw 0:02d2a7571614 768 break;
jebradshaw 0:02d2a7571614 769 }
jebradshaw 3:1892943e3f25 770 }
jebradshaw 0:02d2a7571614 771 }
jebradshaw 0:02d2a7571614 772 }
jebradshaw 0:02d2a7571614 773 }
jebradshaw 3:1892943e3f25 774
jebradshaw 3:1892943e3f25 775 //Trapazoid: move trapazoidal profile on Axis
jebradshaw 1:0d7a1f813571 776 if(c == 'T' || c == 't'){ //move Trapazoid command
jebradshaw 0:02d2a7571614 777 float position = 0.0;
jebradshaw 0:02d2a7571614 778 float time = 0.0;
jebradshaw 0:02d2a7571614 779
jebradshaw 0:02d2a7571614 780 pc.printf("Enter axis to move trapazoid\r\n");
jebradshaw 0:02d2a7571614 781 pc.scanf("%c",&c);
jebradshaw 0:02d2a7571614 782
jebradshaw 0:02d2a7571614 783 pc.printf("\r\n\r\nEnter position:");
jebradshaw 0:02d2a7571614 784 pc.scanf("%f",&position);
jebradshaw 0:02d2a7571614 785 pc.printf("%f\r\n", position);
jebradshaw 0:02d2a7571614 786
jebradshaw 0:02d2a7571614 787 pc.printf("Enter time:");
jebradshaw 0:02d2a7571614 788 pc.scanf("%f",&time);
jebradshaw 0:02d2a7571614 789 pc.printf("%f\r\n", time);
jebradshaw 0:02d2a7571614 790
jebradshaw 0:02d2a7571614 791 switch(c){
jebradshaw 0:02d2a7571614 792 case '1':
jebradshaw 0:02d2a7571614 793 pc.printf("Moving Robotic Axis 1\r\n");
jebradshaw 0:02d2a7571614 794 axis1.moveTrapezoid(position, time);
jebradshaw 0:02d2a7571614 795 break;
jebradshaw 0:02d2a7571614 796
jebradshaw 0:02d2a7571614 797 case '2':
jebradshaw 0:02d2a7571614 798 pc.printf("Moving Robotic Axis 2\r\n");
jebradshaw 3:1892943e3f25 799 axis2.moveTrapezoid(position, time);
jebradshaw 0:02d2a7571614 800 break;
jebradshaw 0:02d2a7571614 801
jebradshaw 0:02d2a7571614 802 case '3':
jebradshaw 0:02d2a7571614 803 pc.printf("Moving Robotic Axis 3\r\n");
jebradshaw 0:02d2a7571614 804 axis3.moveTrapezoid(position, time);
jebradshaw 0:02d2a7571614 805 break;
jebradshaw 0:02d2a7571614 806
jebradshaw 0:02d2a7571614 807 case '4':
jebradshaw 0:02d2a7571614 808 pc.printf("Moving Robotic Axis 4\r\n");
jebradshaw 0:02d2a7571614 809 axis4.moveTrapezoid(position, time);
jebradshaw 0:02d2a7571614 810 break;
jebradshaw 0:02d2a7571614 811
jebradshaw 0:02d2a7571614 812 case '5':
jebradshaw 0:02d2a7571614 813 pc.printf("Moving Robotic Axis 5\r\n");
jebradshaw 0:02d2a7571614 814 axis5.moveTrapezoid(position, time);
jebradshaw 0:02d2a7571614 815 break;
jebradshaw 0:02d2a7571614 816
jebradshaw 0:02d2a7571614 817 case '6':
jebradshaw 0:02d2a7571614 818 pc.printf("Moving Robotic Axis 6\r\n");
jebradshaw 0:02d2a7571614 819 axis6.moveTrapezoid(position, time);
jebradshaw 0:02d2a7571614 820 break;
jebradshaw 0:02d2a7571614 821 }
jebradshaw 3:1892943e3f25 822 }
jebradshaw 3:1892943e3f25 823
jebradshaw 3:1892943e3f25 824 //Home: home command
jebradshaw 1:0d7a1f813571 825 if(c == 'H' || c == 'h'){
jebradshaw 0:02d2a7571614 826 pc.printf("Enter axis to home\r\n");
jebradshaw 0:02d2a7571614 827 pc.scanf("%c",&c);
jebradshaw 0:02d2a7571614 828 switch(c){
jebradshaw 0:02d2a7571614 829 case '1':
jebradshaw 0:02d2a7571614 830 pc.printf("Homing Robotic Axis 1\r\n");
jebradshaw 1:0d7a1f813571 831 axis1.center();
jebradshaw 0:02d2a7571614 832 break;
jebradshaw 0:02d2a7571614 833 case '2':
jebradshaw 1:0d7a1f813571 834 pc.printf("Homing Robotic Axis 2\r\n");
jebradshaw 1:0d7a1f813571 835 axis2.center();
jebradshaw 0:02d2a7571614 836 break;
jebradshaw 1:0d7a1f813571 837
jebradshaw 1:0d7a1f813571 838 case '3':
jebradshaw 1:0d7a1f813571 839 pc.printf("Homing Robotic Axis 3\r\n");
jebradshaw 1:0d7a1f813571 840 axis3.center();
jebradshaw 1:0d7a1f813571 841 break;
jebradshaw 1:0d7a1f813571 842
jebradshaw 1:0d7a1f813571 843 case '4':
jebradshaw 1:0d7a1f813571 844 pc.printf("Homing Robotic Axis 4\r\n");
jebradshaw 1:0d7a1f813571 845 axis4.center();
jebradshaw 1:0d7a1f813571 846 break;
jebradshaw 1:0d7a1f813571 847
jebradshaw 1:0d7a1f813571 848 case '5':
jebradshaw 1:0d7a1f813571 849 pc.printf("Homing Robotic Axis 5\r\n");
jebradshaw 1:0d7a1f813571 850 axis5.center();
jebradshaw 1:0d7a1f813571 851 break;
jebradshaw 1:0d7a1f813571 852
jebradshaw 1:0d7a1f813571 853 case '6':
jebradshaw 1:0d7a1f813571 854 pc.printf("Homing Robotic Axis 6\r\n");
jebradshaw 1:0d7a1f813571 855 axis6.center();
jebradshaw 1:0d7a1f813571 856 break;
jebradshaw 0:02d2a7571614 857 }
jebradshaw 3:1892943e3f25 858 }
jebradshaw 3:1892943e3f25 859
jebradshaw 3:1892943e3f25 860 //Set Point: Manually move to specific encoder position set point
jebradshaw 1:0d7a1f813571 861 if(c == 'S' || c == 's'){
jebradshaw 3:1892943e3f25 862 pc.printf("Enter axis to give set point:");
jebradshaw 1:0d7a1f813571 863 pc.scanf("%c",&c);
jebradshaw 3:1892943e3f25 864 pc.printf("Axis %c entered.\r\n", c);
jebradshaw 1:0d7a1f813571 865 pc.printf("Enter value for set point axis %c\r\n", c);
jebradshaw 3:1892943e3f25 866 float temp_setpoint;
jebradshaw 3:1892943e3f25 867 pc.scanf("%f",&temp_setpoint);
jebradshaw 3:1892943e3f25 868 pc.printf("Axis%c set point %.1f\r\n", c, temp_setpoint);
jebradshaw 3:1892943e3f25 869
jebradshaw 0:02d2a7571614 870 switch(c){
jebradshaw 1:0d7a1f813571 871 case '1':
jebradshaw 3:1892943e3f25 872 axis1.set_point = temp_setpoint;
jebradshaw 3:1892943e3f25 873
jebradshaw 0:02d2a7571614 874 t.reset();
jebradshaw 3:1892943e3f25 875 while((axis1.pos > (axis1.set_point + SP_TOL)) || (axis1.pos < (axis1.set_point - SP_TOL))){
jebradshaw 3:1892943e3f25 876 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 877 wait(.009);
jebradshaw 3:1892943e3f25 878 if(t.read() > 10.0){
jebradshaw 3:1892943e3f25 879 pc.printf("Set point timeout!\r\n");
jebradshaw 3:1892943e3f25 880 break;
jebradshaw 3:1892943e3f25 881 }
jebradshaw 0:02d2a7571614 882 }
jebradshaw 3:1892943e3f25 883 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 884
jebradshaw 0:02d2a7571614 885 break;
jebradshaw 0:02d2a7571614 886
jebradshaw 0:02d2a7571614 887 case '2':
jebradshaw 3:1892943e3f25 888 // float delta3 = axis2.pos - temp_setpoint;
jebradshaw 3:1892943e3f25 889 // axis3.set_point += delta3;
jebradshaw 3:1892943e3f25 890
jebradshaw 3:1892943e3f25 891 axis2.set_point = temp_setpoint;
jebradshaw 0:02d2a7571614 892 t.reset();
jebradshaw 3:1892943e3f25 893 while((axis2.pos > (axis2.set_point + SP_TOL)) || (axis2.pos < (axis2.set_point - SP_TOL))){
jebradshaw 3:1892943e3f25 894 pc.printf("T=%.2f %.3f pos=%.3f vel=%.3f acc=%.3f\r\n", t.read(), axis2.set_point, axis2.pos, axis2.vel, axis2.acc);
jebradshaw 0:02d2a7571614 895 wait(.009);
jebradshaw 3:1892943e3f25 896 if(t.read() > 10.0){
jebradshaw 3:1892943e3f25 897 pc.printf("Set point timeout!\r\n");
jebradshaw 3:1892943e3f25 898 break;
jebradshaw 3:1892943e3f25 899 }
jebradshaw 0:02d2a7571614 900 }
jebradshaw 3:1892943e3f25 901 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 902 break;
jebradshaw 0:02d2a7571614 903
jebradshaw 0:02d2a7571614 904 case '3':
jebradshaw 3:1892943e3f25 905 axis3.set_point = temp_setpoint;
jebradshaw 0:02d2a7571614 906 t.reset();
jebradshaw 3:1892943e3f25 907 while((axis3.pos > (axis3.set_point + SP_TOL)) || (axis3.pos < (axis3.set_point - SP_TOL))){
jebradshaw 3:1892943e3f25 908 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 909 wait(.009);
jebradshaw 3:1892943e3f25 910 if(t.read() > 10.0){
jebradshaw 3:1892943e3f25 911 pc.printf("Set point timeout!\r\n");
jebradshaw 3:1892943e3f25 912 break;
jebradshaw 3:1892943e3f25 913 }
jebradshaw 0:02d2a7571614 914 }
jebradshaw 3:1892943e3f25 915 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 916 break;
jebradshaw 0:02d2a7571614 917
jebradshaw 0:02d2a7571614 918 case '4':
jebradshaw 3:1892943e3f25 919 axis4.set_point = temp_setpoint;
jebradshaw 0:02d2a7571614 920 t.reset();
jebradshaw 3:1892943e3f25 921 while((axis4.pos > (axis4.set_point + SP_TOL)) || (axis4.pos < (axis4.set_point - SP_TOL))){
jebradshaw 3:1892943e3f25 922 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 923 wait(.009);
jebradshaw 3:1892943e3f25 924 if(t.read() > 10.0){
jebradshaw 3:1892943e3f25 925 pc.printf("Set point timeout!\r\n");
jebradshaw 3:1892943e3f25 926 break;
jebradshaw 3:1892943e3f25 927 }
jebradshaw 0:02d2a7571614 928 }
jebradshaw 3:1892943e3f25 929 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 930 break;
jebradshaw 0:02d2a7571614 931
jebradshaw 0:02d2a7571614 932 case '5':
jebradshaw 3:1892943e3f25 933 axis5.set_point = temp_setpoint;
jebradshaw 0:02d2a7571614 934 t.reset();
jebradshaw 3:1892943e3f25 935 while((axis5.pos > (axis5.set_point + SP_TOL)) || (axis5.pos < (axis5.set_point - 50))){
jebradshaw 3:1892943e3f25 936 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 937 wait(.009);
jebradshaw 3:1892943e3f25 938 if(t.read() > 10.0){
jebradshaw 3:1892943e3f25 939 pc.printf("Set point timeout!\r\n");
jebradshaw 3:1892943e3f25 940 break;
jebradshaw 3:1892943e3f25 941 }
jebradshaw 0:02d2a7571614 942 }
jebradshaw 3:1892943e3f25 943 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 944 break;
jebradshaw 0:02d2a7571614 945
jebradshaw 0:02d2a7571614 946 case '6':
jebradshaw 3:1892943e3f25 947 axis6.set_point = temp_setpoint;
jebradshaw 0:02d2a7571614 948 t.reset();
jebradshaw 3:1892943e3f25 949 while((axis6.pos > (axis6.set_point + SP_TOL)) || (axis6.pos < (axis6.set_point - SP_TOL))){
jebradshaw 3:1892943e3f25 950 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 951 wait(.009);
jebradshaw 3:1892943e3f25 952 if(t.read() > 10.0){
jebradshaw 3:1892943e3f25 953 pc.printf("Set point timeout!\r\n");
jebradshaw 3:1892943e3f25 954 break;
jebradshaw 3:1892943e3f25 955 }
jebradshaw 0:02d2a7571614 956 }
jebradshaw 3:1892943e3f25 957 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 958 break;
jebradshaw 0:02d2a7571614 959 }
jebradshaw 0:02d2a7571614 960 }
jebradshaw 3:1892943e3f25 961
jebradshaw 1:0d7a1f813571 962 if(c == 'P' || c == 'p'){
jebradshaw 1:0d7a1f813571 963 float temp_Pk;
jebradshaw 1:0d7a1f813571 964 pc.printf("Enter axis to set Pk\r\n");
jebradshaw 1:0d7a1f813571 965 pc.scanf("%c",&c);
jebradshaw 1:0d7a1f813571 966
jebradshaw 1:0d7a1f813571 967 pc.printf("Enter value for Axis%c Pk\r\n", c);
jebradshaw 1:0d7a1f813571 968 pc.scanf("%f",&temp_Pk);
jebradshaw 1:0d7a1f813571 969
jebradshaw 1:0d7a1f813571 970 switch(c){
jebradshaw 1:0d7a1f813571 971 case 1:
jebradshaw 1:0d7a1f813571 972 axis1.Pk = temp_Pk;
jebradshaw 1:0d7a1f813571 973 break;
jebradshaw 1:0d7a1f813571 974
jebradshaw 1:0d7a1f813571 975 case 2:
jebradshaw 1:0d7a1f813571 976 axis2.Pk = temp_Pk;
jebradshaw 1:0d7a1f813571 977 break;
jebradshaw 1:0d7a1f813571 978
jebradshaw 1:0d7a1f813571 979 case 3:
jebradshaw 1:0d7a1f813571 980 axis3.Pk = temp_Pk;
jebradshaw 1:0d7a1f813571 981 break;
jebradshaw 1:0d7a1f813571 982
jebradshaw 1:0d7a1f813571 983 case 4:
jebradshaw 1:0d7a1f813571 984 axis4.Pk = temp_Pk;
jebradshaw 1:0d7a1f813571 985 break;
jebradshaw 1:0d7a1f813571 986
jebradshaw 1:0d7a1f813571 987 case 5:
jebradshaw 1:0d7a1f813571 988 axis5.Pk = temp_Pk;
jebradshaw 1:0d7a1f813571 989 break;
jebradshaw 1:0d7a1f813571 990
jebradshaw 1:0d7a1f813571 991 case 6:
jebradshaw 1:0d7a1f813571 992 axis6.Pk = temp_Pk;
jebradshaw 1:0d7a1f813571 993 break;
jebradshaw 1:0d7a1f813571 994 }
jebradshaw 1:0d7a1f813571 995 pc.printf("Axis%c Pk set to %f\r\n", c, temp_Pk);
jebradshaw 0:02d2a7571614 996 }
jebradshaw 1:0d7a1f813571 997 if(c == 'I' || c == 'i'){
jebradshaw 1:0d7a1f813571 998 float temp_Ik;
jebradshaw 1:0d7a1f813571 999 pc.printf("Enter axis to set Ik\r\n");
jebradshaw 1:0d7a1f813571 1000 pc.scanf("%c",&c);
jebradshaw 1:0d7a1f813571 1001
jebradshaw 1:0d7a1f813571 1002 pc.printf("Enter value for Axis%c Ik\r\n", c);
jebradshaw 1:0d7a1f813571 1003 pc.scanf("%f",&temp_Ik);
jebradshaw 1:0d7a1f813571 1004
jebradshaw 1:0d7a1f813571 1005 switch(c){
jebradshaw 1:0d7a1f813571 1006 case 1:
jebradshaw 1:0d7a1f813571 1007 axis1.Ik = temp_Ik;
jebradshaw 1:0d7a1f813571 1008 break;
jebradshaw 1:0d7a1f813571 1009
jebradshaw 1:0d7a1f813571 1010 case 2:
jebradshaw 1:0d7a1f813571 1011 axis2.Ik = temp_Ik;
jebradshaw 1:0d7a1f813571 1012 break;
jebradshaw 1:0d7a1f813571 1013
jebradshaw 1:0d7a1f813571 1014 case 3:
jebradshaw 1:0d7a1f813571 1015 axis3.Ik = temp_Ik;
jebradshaw 1:0d7a1f813571 1016 break;
jebradshaw 1:0d7a1f813571 1017
jebradshaw 1:0d7a1f813571 1018 case 4:
jebradshaw 1:0d7a1f813571 1019 axis4.Ik = temp_Ik;
jebradshaw 1:0d7a1f813571 1020 break;
jebradshaw 1:0d7a1f813571 1021
jebradshaw 1:0d7a1f813571 1022 case 5:
jebradshaw 1:0d7a1f813571 1023 axis5.Ik = temp_Ik;
jebradshaw 1:0d7a1f813571 1024 break;
jebradshaw 1:0d7a1f813571 1025
jebradshaw 1:0d7a1f813571 1026 case 6:
jebradshaw 1:0d7a1f813571 1027 axis6.Ik = temp_Ik;
jebradshaw 1:0d7a1f813571 1028 break;
jebradshaw 1:0d7a1f813571 1029 }
jebradshaw 1:0d7a1f813571 1030 pc.printf("Axis%c Ik set to %f\r\n", c, temp_Ik);
jebradshaw 0:02d2a7571614 1031 }
jebradshaw 1:0d7a1f813571 1032 if(c == 'D' || c == 'd'){
jebradshaw 1:0d7a1f813571 1033 float temp_Dk;
jebradshaw 1:0d7a1f813571 1034 pc.printf("Enter axis to set Dk\r\n");
jebradshaw 1:0d7a1f813571 1035 pc.scanf("%c",&c);
jebradshaw 1:0d7a1f813571 1036
jebradshaw 1:0d7a1f813571 1037 pc.printf("Enter value for Axis%c Dk\r\n", c);
jebradshaw 1:0d7a1f813571 1038 pc.scanf("%f",&temp_Dk);
jebradshaw 1:0d7a1f813571 1039
jebradshaw 1:0d7a1f813571 1040 switch(c){
jebradshaw 1:0d7a1f813571 1041 case 1:
jebradshaw 1:0d7a1f813571 1042 axis1.Dk = temp_Dk;
jebradshaw 1:0d7a1f813571 1043 break;
jebradshaw 1:0d7a1f813571 1044
jebradshaw 1:0d7a1f813571 1045 case 2:
jebradshaw 1:0d7a1f813571 1046 axis2.Dk = temp_Dk;
jebradshaw 1:0d7a1f813571 1047 break;
jebradshaw 1:0d7a1f813571 1048
jebradshaw 1:0d7a1f813571 1049 case 3:
jebradshaw 1:0d7a1f813571 1050 axis3.Dk = temp_Dk;
jebradshaw 1:0d7a1f813571 1051 break;
jebradshaw 1:0d7a1f813571 1052
jebradshaw 1:0d7a1f813571 1053 case 4:
jebradshaw 1:0d7a1f813571 1054 axis4.Dk = temp_Dk;
jebradshaw 1:0d7a1f813571 1055 break;
jebradshaw 1:0d7a1f813571 1056
jebradshaw 1:0d7a1f813571 1057 case 5:
jebradshaw 1:0d7a1f813571 1058 axis5.Dk = temp_Dk;
jebradshaw 1:0d7a1f813571 1059 break;
jebradshaw 1:0d7a1f813571 1060
jebradshaw 1:0d7a1f813571 1061 case 6:
jebradshaw 1:0d7a1f813571 1062 axis6.Dk = temp_Dk;
jebradshaw 1:0d7a1f813571 1063 break;
jebradshaw 1:0d7a1f813571 1064 }
jebradshaw 1:0d7a1f813571 1065 pc.printf("Axis%c Ik set to %f\r\n", c, temp_Dk);
jebradshaw 0:02d2a7571614 1066 }
jebradshaw 3:1892943e3f25 1067
jebradshaw 3:1892943e3f25 1068 //Zero: Zero specific axis
jebradshaw 1:0d7a1f813571 1069 if(c == 'Z' || c == 'z'){
jebradshaw 3:1892943e3f25 1070 pc.printf("Enter axis to Zero (1-6, or 'a' for all)\r\n");
jebradshaw 0:02d2a7571614 1071 pc.scanf("%c",&c);
jebradshaw 0:02d2a7571614 1072 switch(c){
jebradshaw 0:02d2a7571614 1073 case '1':
jebradshaw 3:1892943e3f25 1074 axis1.zero();
jebradshaw 0:02d2a7571614 1075 break;
jebradshaw 0:02d2a7571614 1076
jebradshaw 0:02d2a7571614 1077 case '2':
jebradshaw 3:1892943e3f25 1078 axis2.zero();
jebradshaw 0:02d2a7571614 1079 break;
jebradshaw 0:02d2a7571614 1080
jebradshaw 0:02d2a7571614 1081 case '3':
jebradshaw 3:1892943e3f25 1082 axis3.zero();
jebradshaw 0:02d2a7571614 1083 break;
jebradshaw 0:02d2a7571614 1084
jebradshaw 0:02d2a7571614 1085 case '4':
jebradshaw 3:1892943e3f25 1086 axis4.zero();
jebradshaw 0:02d2a7571614 1087 break;
jebradshaw 0:02d2a7571614 1088
jebradshaw 0:02d2a7571614 1089 case '5':
jebradshaw 3:1892943e3f25 1090 axis5.zero();
jebradshaw 0:02d2a7571614 1091 break;
jebradshaw 0:02d2a7571614 1092
jebradshaw 0:02d2a7571614 1093 case '6':
jebradshaw 3:1892943e3f25 1094 axis6.zero();
jebradshaw 3:1892943e3f25 1095 break;
jebradshaw 3:1892943e3f25 1096
jebradshaw 3:1892943e3f25 1097 case 'a': //for all
jebradshaw 3:1892943e3f25 1098 axis1.zero();
jebradshaw 3:1892943e3f25 1099 axis2.zero();
jebradshaw 3:1892943e3f25 1100 axis3.zero();
jebradshaw 3:1892943e3f25 1101 axis4.zero();
jebradshaw 3:1892943e3f25 1102 axis5.zero();
jebradshaw 3:1892943e3f25 1103 axis6.zero();
jebradshaw 3:1892943e3f25 1104 break;
jebradshaw 0:02d2a7571614 1105 }
jebradshaw 0:02d2a7571614 1106
jebradshaw 0:02d2a7571614 1107 }
jebradshaw 1:0d7a1f813571 1108 if(c == 'O' || c == 'o'){
jebradshaw 3:1892943e3f25 1109 pc.printf("Enter axis to turn On (1-6, or 'a' for all)\r\n");
jebradshaw 1:0d7a1f813571 1110 pc.scanf("%c",&c);
jebradshaw 1:0d7a1f813571 1111
jebradshaw 1:0d7a1f813571 1112 switch(c){
jebradshaw 1:0d7a1f813571 1113 case '1':
jebradshaw 1:0d7a1f813571 1114 axis1.axisOn();
jebradshaw 1:0d7a1f813571 1115 break;
jebradshaw 1:0d7a1f813571 1116
jebradshaw 1:0d7a1f813571 1117 case '2':
jebradshaw 1:0d7a1f813571 1118 axis2.axisOn();
jebradshaw 1:0d7a1f813571 1119 break;
jebradshaw 1:0d7a1f813571 1120
jebradshaw 1:0d7a1f813571 1121 case '3':
jebradshaw 1:0d7a1f813571 1122 axis3.axisOn();
jebradshaw 1:0d7a1f813571 1123 break;
jebradshaw 1:0d7a1f813571 1124
jebradshaw 1:0d7a1f813571 1125 case '4':
jebradshaw 1:0d7a1f813571 1126 axis4.axisOn();
jebradshaw 1:0d7a1f813571 1127 break;
jebradshaw 1:0d7a1f813571 1128
jebradshaw 1:0d7a1f813571 1129 case '5':
jebradshaw 1:0d7a1f813571 1130 axis5.axisOn();
jebradshaw 1:0d7a1f813571 1131 break;
jebradshaw 1:0d7a1f813571 1132
jebradshaw 1:0d7a1f813571 1133 case '6':
jebradshaw 1:0d7a1f813571 1134 axis6.axisOn();
jebradshaw 3:1892943e3f25 1135 break;
jebradshaw 3:1892943e3f25 1136
jebradshaw 3:1892943e3f25 1137 case 'a':
jebradshaw 3:1892943e3f25 1138 axis1.axisOn();
jebradshaw 3:1892943e3f25 1139 axis2.axisOn();
jebradshaw 3:1892943e3f25 1140 axis3.axisOn();
jebradshaw 3:1892943e3f25 1141 axis4.axisOn();
jebradshaw 3:1892943e3f25 1142 axis5.axisOn();
jebradshaw 3:1892943e3f25 1143 axis6.axisOn();
jebradshaw 3:1892943e3f25 1144 break;
jebradshaw 1:0d7a1f813571 1145 }
jebradshaw 3:1892943e3f25 1146 pc.printf("Axis%c On\r\n", c);
jebradshaw 1:0d7a1f813571 1147 }
jebradshaw 1:0d7a1f813571 1148 if(c == 'F' || c == 'f'){
jebradshaw 3:1892943e3f25 1149 pc.printf("Enter axis to turn Off (1-6, or 'a' for all)\r\n");
jebradshaw 1:0d7a1f813571 1150 pc.scanf("%c",&c);
jebradshaw 0:02d2a7571614 1151
jebradshaw 1:0d7a1f813571 1152 switch(c){
jebradshaw 1:0d7a1f813571 1153 case '1':
jebradshaw 1:0d7a1f813571 1154 axis1.axisOff();
jebradshaw 1:0d7a1f813571 1155 break;
jebradshaw 1:0d7a1f813571 1156
jebradshaw 1:0d7a1f813571 1157 case '2':
jebradshaw 1:0d7a1f813571 1158 axis2.axisOff();
jebradshaw 1:0d7a1f813571 1159 break;
jebradshaw 1:0d7a1f813571 1160
jebradshaw 1:0d7a1f813571 1161 case '3':
jebradshaw 1:0d7a1f813571 1162 axis3.axisOff();
jebradshaw 1:0d7a1f813571 1163 break;
jebradshaw 1:0d7a1f813571 1164
jebradshaw 1:0d7a1f813571 1165 case '4':
jebradshaw 1:0d7a1f813571 1166 axis4.axisOff();
jebradshaw 1:0d7a1f813571 1167 break;
jebradshaw 1:0d7a1f813571 1168
jebradshaw 1:0d7a1f813571 1169 case '5':
jebradshaw 1:0d7a1f813571 1170 axis5.axisOff();
jebradshaw 1:0d7a1f813571 1171 break;
jebradshaw 1:0d7a1f813571 1172
jebradshaw 1:0d7a1f813571 1173 case '6':
jebradshaw 1:0d7a1f813571 1174 axis6.axisOff();
jebradshaw 3:1892943e3f25 1175 break;
jebradshaw 3:1892943e3f25 1176
jebradshaw 3:1892943e3f25 1177 case 'a':
jebradshaw 3:1892943e3f25 1178 axis1.axisOff();
jebradshaw 3:1892943e3f25 1179 axis2.axisOff();
jebradshaw 3:1892943e3f25 1180 axis3.axisOff();
jebradshaw 3:1892943e3f25 1181 axis4.axisOff();
jebradshaw 3:1892943e3f25 1182 axis5.axisOff();
jebradshaw 3:1892943e3f25 1183 axis6.axisOff();
jebradshaw 3:1892943e3f25 1184 break;
jebradshaw 1:0d7a1f813571 1185 }
jebradshaw 3:1892943e3f25 1186 pc.printf("Axis%c Off\r\n", c);
jebradshaw 3:1892943e3f25 1187 }
jebradshaw 3:1892943e3f25 1188
jebradshaw 3:1892943e3f25 1189 // Toggle Stream flag (for display purposes
jebradshaw 3:1892943e3f25 1190 if(c == 'J' || c == 'j'){
jebradshaw 3:1892943e3f25 1191 pc.printf("Enter 1 to turn stream On, 0 to turn Off:\r\n");
jebradshaw 3:1892943e3f25 1192 pc.scanf("%c",&c);
jebradshaw 1:0d7a1f813571 1193
jebradshaw 3:1892943e3f25 1194 if(c == '1'){
jebradshaw 3:1892943e3f25 1195 streamFlag = 1;
jebradshaw 3:1892943e3f25 1196 pc.printf("Stream On\r\n");
jebradshaw 3:1892943e3f25 1197 }
jebradshaw 3:1892943e3f25 1198 if(c == '0'){
jebradshaw 3:1892943e3f25 1199 streamFlag = 0;
jebradshaw 3:1892943e3f25 1200 pc.printf("Stream Off\r\n");
jebradshaw 3:1892943e3f25 1201 }
jebradshaw 3:1892943e3f25 1202 c = '\0';
jebradshaw 1:0d7a1f813571 1203 }
jebradshaw 3:1892943e3f25 1204
jebradshaw 3:1892943e3f25 1205 if(c == 'M' || c == 'm'){ //move axis (with joints combined)
jebradshaw 3:1892943e3f25 1206 pc.printf("Enter axis to move\r\n");
jebradshaw 3:1892943e3f25 1207 pc.scanf("%c",&c);
jebradshaw 3:1892943e3f25 1208 pc.printf("Enter value for axis %c\r\n", c);
jebradshaw 3:1892943e3f25 1209 float newPosition;
jebradshaw 3:1892943e3f25 1210 switch(c){
jebradshaw 3:1892943e3f25 1211 case '2':
jebradshaw 3:1892943e3f25 1212 pc.scanf("%f",&newPosition);
jebradshaw 3:1892943e3f25 1213 axis2.set_point += newPosition;
jebradshaw 3:1892943e3f25 1214 axis3.set_point += newPosition;
jebradshaw 3:1892943e3f25 1215 axis4.set_point += (65.5/127.0 * newPosition);
jebradshaw 3:1892943e3f25 1216 axis5.set_point -= (65.5/127.0 * newPosition);
jebradshaw 3:1892943e3f25 1217
jebradshaw 3:1892943e3f25 1218
jebradshaw 3:1892943e3f25 1219 t.reset();
jebradshaw 3:1892943e3f25 1220 while((axis1.pos > (axis1.set_point + SP_TOL)) || (axis1.pos < (axis1.set_point - SP_TOL))){
jebradshaw 3:1892943e3f25 1221 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 3:1892943e3f25 1222 wait(.009);
jebradshaw 3:1892943e3f25 1223 if(t.read() > 10.0){
jebradshaw 3:1892943e3f25 1224 pc.printf("Set point timeout!\r\n");
jebradshaw 3:1892943e3f25 1225 break;
jebradshaw 3:1892943e3f25 1226 }
jebradshaw 3:1892943e3f25 1227 }
jebradshaw 3:1892943e3f25 1228 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 3:1892943e3f25 1229
jebradshaw 3:1892943e3f25 1230 break;
jebradshaw 3:1892943e3f25 1231 }
jebradshaw 3:1892943e3f25 1232 }
jebradshaw 1:0d7a1f813571 1233 }//command was received
jebradshaw 0:02d2a7571614 1234
jebradshaw 0:02d2a7571614 1235 if((axis1.moveState==0) && (axis2.moveState==0) && (axis3.moveState==0) && (axis4.moveState==0) && (axis5.moveState==0) && (axis6.moveState==0)){
jebradshaw 3:1892943e3f25 1236 pc.printf("%.2f,%.1f,%.1f,%.1f,%.1f,%.1f,%.1f,%.1f,%.1f,%.1f,%.1f,%.1f,%.1f \r", (float)t.read(), (float)axis1.set_point, (float)axis1.pos, (float)axis2.set_point, (float)axis2.pos,
jebradshaw 3:1892943e3f25 1237 (float)axis3.set_point, (float)axis3.pos, (float)axis4.set_point, (float)axis4.pos,(float)axis5.set_point, (float)axis5.pos, (float)axis6.set_point, (float)axis6.pos);
jebradshaw 3:1892943e3f25 1238 if(streamFlag)
jebradshaw 3:1892943e3f25 1239 pc.printf("\n");
jebradshaw 0:02d2a7571614 1240 led2 = 0;
jebradshaw 0:02d2a7571614 1241 }
jebradshaw 0:02d2a7571614 1242 else
jebradshaw 0:02d2a7571614 1243 led2 = 1;
jebradshaw 3:1892943e3f25 1244
jebradshaw 3:1892943e3f25 1245 // pc.printf("Axis2: pos=%.1f co=%.1f setPoint=%.1f vel=%.1f\r\n", axis2.pos, axis2.co, axis2.set_point, axis2.vel);
jebradshaw 0:02d2a7571614 1246 wait(.02);
jebradshaw 0:02d2a7571614 1247 }//while(1)
jebradshaw 0:02d2a7571614 1248 }//main
jebradshaw 1:0d7a1f813571 1249