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:
Thu Dec 17 19:47:39 2015 +0000
Revision:
3:1892943e3f25
Parent:
2:e5d56ee55af6
Child:
4:890c104546e9
6 axis arm controller implemented on LPC1768 micro.  .bin is generated using mbed compiler, IC is programmed using bin2hex converted and FlashMagic for programming hex file.

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