Serial interface for controlling robotic arm.
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.
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!!
Express PCB Artwork
Inner Power Layer Breakup for motor current
First Prototype
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
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
lpc_axis.cpp@4:890c104546e9, 2015-12-28 (annotated)
- 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?
User | Revision | Line number | New 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 |