Program for driving bipolar stepper motor model 17HA0403-32N. Motor can be driven in wave, full step, and half step modes in clockwise and anticlockwise directions.
Fork of LVHB Stepper Motor Drive by
main.cpp@0:d14cf1e75576, 2014-11-14 (annotated)
- Committer:
- bdbohn
- Date:
- Fri Nov 14 21:03:06 2014 +0000
- Revision:
- 0:d14cf1e75576
- Child:
- 1:48b0212c91e9
Initial Releas
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
bdbohn | 0:d14cf1e75576 | 1 | |
bdbohn | 0:d14cf1e75576 | 2 | //Libraries needed |
bdbohn | 0:d14cf1e75576 | 3 | #include "mbed.h" |
bdbohn | 0:d14cf1e75576 | 4 | #include "USBHID.h" |
bdbohn | 0:d14cf1e75576 | 5 | |
bdbohn | 0:d14cf1e75576 | 6 | // We declare a USBHID device. |
bdbohn | 0:d14cf1e75576 | 7 | // HID In/Out Reports are 64 Bytes long |
bdbohn | 0:d14cf1e75576 | 8 | // Vendor ID (VID): 0x15A2 |
bdbohn | 0:d14cf1e75576 | 9 | // Product ID (PID): 0x0138 |
bdbohn | 0:d14cf1e75576 | 10 | // Serial Number: 0x0001 |
bdbohn | 0:d14cf1e75576 | 11 | USBHID hid(64, 64, 0x15A2, 0x0138, 0x0001, true); |
bdbohn | 0:d14cf1e75576 | 12 | |
bdbohn | 0:d14cf1e75576 | 13 | //storage for send and receive data |
bdbohn | 0:d14cf1e75576 | 14 | HID_REPORT send_report; |
bdbohn | 0:d14cf1e75576 | 15 | HID_REPORT recv_report; |
bdbohn | 0:d14cf1e75576 | 16 | |
bdbohn | 0:d14cf1e75576 | 17 | //Stepper Motor States |
bdbohn | 0:d14cf1e75576 | 18 | #define STATE1 1 |
bdbohn | 0:d14cf1e75576 | 19 | #define STATE2 2 |
bdbohn | 0:d14cf1e75576 | 20 | #define STATE3 3 |
bdbohn | 0:d14cf1e75576 | 21 | #define STATE4 4 |
bdbohn | 0:d14cf1e75576 | 22 | #define STATE5 5 |
bdbohn | 0:d14cf1e75576 | 23 | #define STATE6 6 |
bdbohn | 0:d14cf1e75576 | 24 | #define STATE7 7 |
bdbohn | 0:d14cf1e75576 | 25 | #define STATE8 8 |
bdbohn | 0:d14cf1e75576 | 26 | |
bdbohn | 0:d14cf1e75576 | 27 | // USB COMMANDS |
bdbohn | 0:d14cf1e75576 | 28 | // These are sent from the PC |
bdbohn | 0:d14cf1e75576 | 29 | #define WRITE_LED 0x20 |
bdbohn | 0:d14cf1e75576 | 30 | #define WRITE_OUTPUT_EN 0x30 |
bdbohn | 0:d14cf1e75576 | 31 | #define WRITE_STEPS_PER_SEC 0x40 |
bdbohn | 0:d14cf1e75576 | 32 | #define WRITE_RUN_STOP 0x70 |
bdbohn | 0:d14cf1e75576 | 33 | #define WRITE_DIRECTION 0x71 |
bdbohn | 0:d14cf1e75576 | 34 | #define WRITE_ACCEL 0x80 |
bdbohn | 0:d14cf1e75576 | 35 | #define WRITE_RESET 0xA0 |
bdbohn | 0:d14cf1e75576 | 36 | #define WRITE_STEPMODE 0xB0 |
bdbohn | 0:d14cf1e75576 | 37 | |
bdbohn | 0:d14cf1e75576 | 38 | |
bdbohn | 0:d14cf1e75576 | 39 | // MOTOR STATES |
bdbohn | 0:d14cf1e75576 | 40 | #define STOP 0x00 |
bdbohn | 0:d14cf1e75576 | 41 | #define RUN 0x02 |
bdbohn | 0:d14cf1e75576 | 42 | #define RAMP 0x03 |
bdbohn | 0:d14cf1e75576 | 43 | #define RESET 0x05 |
bdbohn | 0:d14cf1e75576 | 44 | |
bdbohn | 0:d14cf1e75576 | 45 | // LED CONSTANTS |
bdbohn | 0:d14cf1e75576 | 46 | #define LEDS_OFF 0x00 |
bdbohn | 0:d14cf1e75576 | 47 | #define RED 0x01 |
bdbohn | 0:d14cf1e75576 | 48 | #define GREEN 0x02 |
bdbohn | 0:d14cf1e75576 | 49 | #define BLUE 0x03 |
bdbohn | 0:d14cf1e75576 | 50 | #define READY_LED 0x04 |
bdbohn | 0:d14cf1e75576 | 51 | |
bdbohn | 0:d14cf1e75576 | 52 | // LOGICAL CONSTANTS |
bdbohn | 0:d14cf1e75576 | 53 | #define OFF 0x00 |
bdbohn | 0:d14cf1e75576 | 54 | #define ON 0x01 |
bdbohn | 0:d14cf1e75576 | 55 | |
bdbohn | 0:d14cf1e75576 | 56 | //step modes |
bdbohn | 0:d14cf1e75576 | 57 | #define QTR 0x01 |
bdbohn | 0:d14cf1e75576 | 58 | #define HALF 0x02 |
bdbohn | 0:d14cf1e75576 | 59 | |
bdbohn | 0:d14cf1e75576 | 60 | //FRDM-KL25Z LEDs |
bdbohn | 0:d14cf1e75576 | 61 | DigitalOut red_led(LED1); |
bdbohn | 0:d14cf1e75576 | 62 | DigitalOut green_led(LED2); |
bdbohn | 0:d14cf1e75576 | 63 | DigitalOut blue_led(LED3); |
bdbohn | 0:d14cf1e75576 | 64 | |
bdbohn | 0:d14cf1e75576 | 65 | //Input pins on Eval Board |
bdbohn | 0:d14cf1e75576 | 66 | DigitalOut IN1A(PTD4); // Pin IN1A input to EVAL board (FRDM PIN Name) |
bdbohn | 0:d14cf1e75576 | 67 | DigitalOut IN1B(PTA12); // Pin IN1B input to EVAL board (FRDM PIN Name) |
bdbohn | 0:d14cf1e75576 | 68 | DigitalOut IN2A(PTA4); // Pin IN2A input to EVAL board (FRDM PIN Name) |
bdbohn | 0:d14cf1e75576 | 69 | DigitalOut IN2B(PTA5); // Pin IN2B input to EVAL board (FRDM PIN Name) |
bdbohn | 0:d14cf1e75576 | 70 | |
bdbohn | 0:d14cf1e75576 | 71 | //Green LED on Eval Board |
bdbohn | 0:d14cf1e75576 | 72 | DigitalOut READY(PTC5); // Pin READY input to EVAL board (FRDM PIN Name) |
bdbohn | 0:d14cf1e75576 | 73 | |
bdbohn | 0:d14cf1e75576 | 74 | //These pins are defined differntly on different parts |
bdbohn | 0:d14cf1e75576 | 75 | //OE for FRDM-17529 and FRDM-17533 Eval Boards |
bdbohn | 0:d14cf1e75576 | 76 | //and PSAVE for FRDM-17C724 and FRDM-17531 Eval Boards |
bdbohn | 0:d14cf1e75576 | 77 | //FRDM-34933 Eval Board does not have either of these pins |
bdbohn | 0:d14cf1e75576 | 78 | DigitalOut OE(PTC7); // Pin OE input to MPC17533, MPC17529 (FRDM PIN Name) |
bdbohn | 0:d14cf1e75576 | 79 | |
bdbohn | 0:d14cf1e75576 | 80 | //variables |
bdbohn | 0:d14cf1e75576 | 81 | static int stepState = 1; //holds current step state of the step being applied to the stepper |
bdbohn | 0:d14cf1e75576 | 82 | static int stepMode = 0; //This is either 1/2 or 1/4 step |
bdbohn | 0:d14cf1e75576 | 83 | static int dir = 0; //rotational direction of stepper |
bdbohn | 0:d14cf1e75576 | 84 | static int rampCount = 0; //counter value used during acceleration ramp-up |
bdbohn | 0:d14cf1e75576 | 85 | static int initflag = 0; //used upon first entry into main at startup |
bdbohn | 0:d14cf1e75576 | 86 | static int runstop = 0; //holds value of running or stopped of commanded value from PC |
bdbohn | 0:d14cf1e75576 | 87 | static int motorState = 0; //holds state of stepper motor state machine (RUN, STOP, RESET, or RAMP) |
bdbohn | 0:d14cf1e75576 | 88 | static int accel = 0; //holds the value of accceleration enabled from PC |
bdbohn | 0:d14cf1e75576 | 89 | static float numRampSteps = 0; //calculated value that is based on the stepper speed after acceleration |
bdbohn | 0:d14cf1e75576 | 90 | static float stepRate = 10; |
bdbohn | 0:d14cf1e75576 | 91 | static int stepMultiplier = 1; |
bdbohn | 0:d14cf1e75576 | 92 | static float next_interval_us = 1200; |
bdbohn | 0:d14cf1e75576 | 93 | static float stepInterval_us = 1200; |
bdbohn | 0:d14cf1e75576 | 94 | static bool acceleration_start = 0; |
bdbohn | 0:d14cf1e75576 | 95 | static bool runStopChange = 0; |
bdbohn | 0:d14cf1e75576 | 96 | static bool accel_wait_flag = 0; |
bdbohn | 0:d14cf1e75576 | 97 | |
bdbohn | 0:d14cf1e75576 | 98 | //static int mode = 0; |
bdbohn | 0:d14cf1e75576 | 99 | |
bdbohn | 0:d14cf1e75576 | 100 | void test_state_mach(void); |
bdbohn | 0:d14cf1e75576 | 101 | void adv_state_mach_half(void); |
bdbohn | 0:d14cf1e75576 | 102 | void adv_state_mach_full(void); |
bdbohn | 0:d14cf1e75576 | 103 | void set_speed(float stepRate); |
bdbohn | 0:d14cf1e75576 | 104 | void acceleration_a(void); |
bdbohn | 0:d14cf1e75576 | 105 | void acceleration_b(void); |
bdbohn | 0:d14cf1e75576 | 106 | void set_speed_with_ramp(void); |
bdbohn | 0:d14cf1e75576 | 107 | void null_function(void); |
bdbohn | 0:d14cf1e75576 | 108 | void clear_accel_wait_flag(void); |
bdbohn | 0:d14cf1e75576 | 109 | |
bdbohn | 0:d14cf1e75576 | 110 | |
bdbohn | 0:d14cf1e75576 | 111 | |
bdbohn | 0:d14cf1e75576 | 112 | // |
bdbohn | 0:d14cf1e75576 | 113 | //Create a Timeout function. Used for ramping up the speed at startup |
bdbohn | 0:d14cf1e75576 | 114 | Timeout accel_ramp; |
bdbohn | 0:d14cf1e75576 | 115 | |
bdbohn | 0:d14cf1e75576 | 116 | //Create a Ticker function. Called during normal runtime to advance the stepper motor |
bdbohn | 0:d14cf1e75576 | 117 | Ticker advance_state; |
bdbohn | 0:d14cf1e75576 | 118 | |
bdbohn | 0:d14cf1e75576 | 119 | |
bdbohn | 0:d14cf1e75576 | 120 | int main() |
bdbohn | 0:d14cf1e75576 | 121 | { |
bdbohn | 0:d14cf1e75576 | 122 | //set up storage for incoming/outgoing |
bdbohn | 0:d14cf1e75576 | 123 | send_report.length = 64; |
bdbohn | 0:d14cf1e75576 | 124 | recv_report.length = 64; |
bdbohn | 0:d14cf1e75576 | 125 | |
bdbohn | 0:d14cf1e75576 | 126 | red_led = 1; // Red LED = OFF |
bdbohn | 0:d14cf1e75576 | 127 | green_led = 1; // Green LED = OFF |
bdbohn | 0:d14cf1e75576 | 128 | blue_led = 0; // Blue LED = ON |
bdbohn | 0:d14cf1e75576 | 129 | |
bdbohn | 0:d14cf1e75576 | 130 | READY = 0; |
bdbohn | 0:d14cf1e75576 | 131 | |
bdbohn | 0:d14cf1e75576 | 132 | motorState = RESET; |
bdbohn | 0:d14cf1e75576 | 133 | |
bdbohn | 0:d14cf1e75576 | 134 | while(1) |
bdbohn | 0:d14cf1e75576 | 135 | { |
bdbohn | 0:d14cf1e75576 | 136 | //try to read a msg |
bdbohn | 0:d14cf1e75576 | 137 | if(hid.readNB(&recv_report)) |
bdbohn | 0:d14cf1e75576 | 138 | { |
bdbohn | 0:d14cf1e75576 | 139 | if(initflag == true) |
bdbohn | 0:d14cf1e75576 | 140 | { |
bdbohn | 0:d14cf1e75576 | 141 | initflag = false; |
bdbohn | 0:d14cf1e75576 | 142 | blue_led = 1; //turn off blue LED |
bdbohn | 0:d14cf1e75576 | 143 | READY = 0; |
bdbohn | 0:d14cf1e75576 | 144 | } |
bdbohn | 0:d14cf1e75576 | 145 | switch(recv_report.data[0]) //byte 0 of recv_report.data is command |
bdbohn | 0:d14cf1e75576 | 146 | { |
bdbohn | 0:d14cf1e75576 | 147 | //----------------------------------------------------------------------------------------------------------------- |
bdbohn | 0:d14cf1e75576 | 148 | // COMMAND PARSER |
bdbohn | 0:d14cf1e75576 | 149 | //----------------------------------------------------------------------------------------------------------------- |
bdbohn | 0:d14cf1e75576 | 150 | //////// |
bdbohn | 0:d14cf1e75576 | 151 | case WRITE_LED: |
bdbohn | 0:d14cf1e75576 | 152 | switch(recv_report.data[1]) |
bdbohn | 0:d14cf1e75576 | 153 | { |
bdbohn | 0:d14cf1e75576 | 154 | case LEDS_OFF: |
bdbohn | 0:d14cf1e75576 | 155 | red_led = 1; |
bdbohn | 0:d14cf1e75576 | 156 | green_led = 1; |
bdbohn | 0:d14cf1e75576 | 157 | blue_led = 1; |
bdbohn | 0:d14cf1e75576 | 158 | break; |
bdbohn | 0:d14cf1e75576 | 159 | case RED: |
bdbohn | 0:d14cf1e75576 | 160 | if(recv_report.data[2] == 1){red_led = 0;} else {red_led = 1;} |
bdbohn | 0:d14cf1e75576 | 161 | break; |
bdbohn | 0:d14cf1e75576 | 162 | case GREEN: |
bdbohn | 0:d14cf1e75576 | 163 | if(recv_report.data[2] == 1){green_led = 0;} else {green_led = 1;} |
bdbohn | 0:d14cf1e75576 | 164 | break; |
bdbohn | 0:d14cf1e75576 | 165 | case BLUE: |
bdbohn | 0:d14cf1e75576 | 166 | if(recv_report.data[2] == 1){blue_led = 0;} else {blue_led = 1;} |
bdbohn | 0:d14cf1e75576 | 167 | break; |
bdbohn | 0:d14cf1e75576 | 168 | default: |
bdbohn | 0:d14cf1e75576 | 169 | break; |
bdbohn | 0:d14cf1e75576 | 170 | }// End recv report data[1] |
bdbohn | 0:d14cf1e75576 | 171 | break; |
bdbohn | 0:d14cf1e75576 | 172 | ////////end case WRITE_LED |
bdbohn | 0:d14cf1e75576 | 173 | |
bdbohn | 0:d14cf1e75576 | 174 | //////// |
bdbohn | 0:d14cf1e75576 | 175 | case WRITE_STEPS_PER_SEC: |
bdbohn | 0:d14cf1e75576 | 176 | |
bdbohn | 0:d14cf1e75576 | 177 | stepRate = recv_report.data[1]; |
bdbohn | 0:d14cf1e75576 | 178 | set_speed(stepRate); |
bdbohn | 0:d14cf1e75576 | 179 | break; |
bdbohn | 0:d14cf1e75576 | 180 | //////// |
bdbohn | 0:d14cf1e75576 | 181 | case WRITE_RUN_STOP: |
bdbohn | 0:d14cf1e75576 | 182 | if(recv_report.data[1] == 1) |
bdbohn | 0:d14cf1e75576 | 183 | { |
bdbohn | 0:d14cf1e75576 | 184 | if(runstop != 1) |
bdbohn | 0:d14cf1e75576 | 185 | { |
bdbohn | 0:d14cf1e75576 | 186 | red_led = 1; |
bdbohn | 0:d14cf1e75576 | 187 | blue_led = 1; |
bdbohn | 0:d14cf1e75576 | 188 | green_led = 0; |
bdbohn | 0:d14cf1e75576 | 189 | runstop = 1; |
bdbohn | 0:d14cf1e75576 | 190 | runStopChange = 1; |
bdbohn | 0:d14cf1e75576 | 191 | |
bdbohn | 0:d14cf1e75576 | 192 | if(accel == 1) |
bdbohn | 0:d14cf1e75576 | 193 | { |
bdbohn | 0:d14cf1e75576 | 194 | motorState = RAMP; |
bdbohn | 0:d14cf1e75576 | 195 | next_interval_us = stepMultiplier; |
bdbohn | 0:d14cf1e75576 | 196 | acceleration_start = 1; |
bdbohn | 0:d14cf1e75576 | 197 | } |
bdbohn | 0:d14cf1e75576 | 198 | else |
bdbohn | 0:d14cf1e75576 | 199 | { |
bdbohn | 0:d14cf1e75576 | 200 | motorState = RUN; |
bdbohn | 0:d14cf1e75576 | 201 | set_speed(stepRate); |
bdbohn | 0:d14cf1e75576 | 202 | } |
bdbohn | 0:d14cf1e75576 | 203 | } |
bdbohn | 0:d14cf1e75576 | 204 | |
bdbohn | 0:d14cf1e75576 | 205 | } |
bdbohn | 0:d14cf1e75576 | 206 | else |
bdbohn | 0:d14cf1e75576 | 207 | { |
bdbohn | 0:d14cf1e75576 | 208 | if(runstop != 0) |
bdbohn | 0:d14cf1e75576 | 209 | { |
bdbohn | 0:d14cf1e75576 | 210 | runstop = 0; |
bdbohn | 0:d14cf1e75576 | 211 | runStopChange = 1; |
bdbohn | 0:d14cf1e75576 | 212 | motorState = STOP; |
bdbohn | 0:d14cf1e75576 | 213 | red_led = 0; |
bdbohn | 0:d14cf1e75576 | 214 | green_led = 1; |
bdbohn | 0:d14cf1e75576 | 215 | blue_led = 1; |
bdbohn | 0:d14cf1e75576 | 216 | } |
bdbohn | 0:d14cf1e75576 | 217 | }//end if(recv_report.data[1] == 1) |
bdbohn | 0:d14cf1e75576 | 218 | break; |
bdbohn | 0:d14cf1e75576 | 219 | //////// |
bdbohn | 0:d14cf1e75576 | 220 | case WRITE_DIRECTION: |
bdbohn | 0:d14cf1e75576 | 221 | |
bdbohn | 0:d14cf1e75576 | 222 | if(recv_report.data[1] == 1) |
bdbohn | 0:d14cf1e75576 | 223 | { |
bdbohn | 0:d14cf1e75576 | 224 | dir = 1; |
bdbohn | 0:d14cf1e75576 | 225 | |
bdbohn | 0:d14cf1e75576 | 226 | } |
bdbohn | 0:d14cf1e75576 | 227 | else |
bdbohn | 0:d14cf1e75576 | 228 | { |
bdbohn | 0:d14cf1e75576 | 229 | dir = 0; |
bdbohn | 0:d14cf1e75576 | 230 | |
bdbohn | 0:d14cf1e75576 | 231 | } |
bdbohn | 0:d14cf1e75576 | 232 | break; |
bdbohn | 0:d14cf1e75576 | 233 | //////// |
bdbohn | 0:d14cf1e75576 | 234 | case WRITE_ACCEL: |
bdbohn | 0:d14cf1e75576 | 235 | if(recv_report.data[1] == 1) |
bdbohn | 0:d14cf1e75576 | 236 | { |
bdbohn | 0:d14cf1e75576 | 237 | accel = 1; |
bdbohn | 0:d14cf1e75576 | 238 | } |
bdbohn | 0:d14cf1e75576 | 239 | else |
bdbohn | 0:d14cf1e75576 | 240 | { |
bdbohn | 0:d14cf1e75576 | 241 | accel = 0; |
bdbohn | 0:d14cf1e75576 | 242 | } |
bdbohn | 0:d14cf1e75576 | 243 | break; |
bdbohn | 0:d14cf1e75576 | 244 | //////// |
bdbohn | 0:d14cf1e75576 | 245 | case WRITE_STEPMODE: |
bdbohn | 0:d14cf1e75576 | 246 | if(recv_report.data[1] == QTR) |
bdbohn | 0:d14cf1e75576 | 247 | { |
bdbohn | 0:d14cf1e75576 | 248 | stepMode = QTR; |
bdbohn | 0:d14cf1e75576 | 249 | stepMultiplier = 8; |
bdbohn | 0:d14cf1e75576 | 250 | |
bdbohn | 0:d14cf1e75576 | 251 | } |
bdbohn | 0:d14cf1e75576 | 252 | else |
bdbohn | 0:d14cf1e75576 | 253 | { |
bdbohn | 0:d14cf1e75576 | 254 | stepMode = HALF; |
bdbohn | 0:d14cf1e75576 | 255 | stepMultiplier = 4; |
bdbohn | 0:d14cf1e75576 | 256 | |
bdbohn | 0:d14cf1e75576 | 257 | } |
bdbohn | 0:d14cf1e75576 | 258 | break; |
bdbohn | 0:d14cf1e75576 | 259 | //////// |
bdbohn | 0:d14cf1e75576 | 260 | default: |
bdbohn | 0:d14cf1e75576 | 261 | break; |
bdbohn | 0:d14cf1e75576 | 262 | }// End Switch recv report data[0] |
bdbohn | 0:d14cf1e75576 | 263 | |
bdbohn | 0:d14cf1e75576 | 264 | |
bdbohn | 0:d14cf1e75576 | 265 | //----------------------------------------------------------------------------------------------------------------- |
bdbohn | 0:d14cf1e75576 | 266 | // end command parser |
bdbohn | 0:d14cf1e75576 | 267 | //----------------------------------------------------------------------------------------------------------------- |
bdbohn | 0:d14cf1e75576 | 268 | |
bdbohn | 0:d14cf1e75576 | 269 | send_report.data[0] = recv_report.data[0]; // Echo Command |
bdbohn | 0:d14cf1e75576 | 270 | send_report.data[1] = recv_report.data[1]; // Echo Subcommand 1 |
bdbohn | 0:d14cf1e75576 | 271 | send_report.data[2] = recv_report.data[2]; // Echo Subcommand 2 |
bdbohn | 0:d14cf1e75576 | 272 | send_report.data[3] = 0x00; |
bdbohn | 0:d14cf1e75576 | 273 | send_report.data[4] = 0x00; |
bdbohn | 0:d14cf1e75576 | 274 | send_report.data[5] = 0x00; |
bdbohn | 0:d14cf1e75576 | 275 | send_report.data[6] = 0x00; |
bdbohn | 0:d14cf1e75576 | 276 | send_report.data[7] = 0x00; |
bdbohn | 0:d14cf1e75576 | 277 | |
bdbohn | 0:d14cf1e75576 | 278 | //Send the report |
bdbohn | 0:d14cf1e75576 | 279 | hid.send(&send_report); |
bdbohn | 0:d14cf1e75576 | 280 | }// End If(hid.readNB(&recv_report)) |
bdbohn | 0:d14cf1e75576 | 281 | ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
bdbohn | 0:d14cf1e75576 | 282 | //End of USB message handling |
bdbohn | 0:d14cf1e75576 | 283 | ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
bdbohn | 0:d14cf1e75576 | 284 | |
bdbohn | 0:d14cf1e75576 | 285 | |
bdbohn | 0:d14cf1e75576 | 286 | /************************************************************************************************************************ |
bdbohn | 0:d14cf1e75576 | 287 | // This is handling of Speed, Acceleration, Direction, and Start/Stop |
bdbohn | 0:d14cf1e75576 | 288 | ***********************************************************************************************************************/ |
bdbohn | 0:d14cf1e75576 | 289 | |
bdbohn | 0:d14cf1e75576 | 290 | |
bdbohn | 0:d14cf1e75576 | 291 | |
bdbohn | 0:d14cf1e75576 | 292 | |
bdbohn | 0:d14cf1e75576 | 293 | |
bdbohn | 0:d14cf1e75576 | 294 | |
bdbohn | 0:d14cf1e75576 | 295 | switch (motorState) |
bdbohn | 0:d14cf1e75576 | 296 | { |
bdbohn | 0:d14cf1e75576 | 297 | case STOP: |
bdbohn | 0:d14cf1e75576 | 298 | if(runStopChange == 1) |
bdbohn | 0:d14cf1e75576 | 299 | { |
bdbohn | 0:d14cf1e75576 | 300 | runStopChange = 0; |
bdbohn | 0:d14cf1e75576 | 301 | OE = 1; |
bdbohn | 0:d14cf1e75576 | 302 | advance_state.detach(); |
bdbohn | 0:d14cf1e75576 | 303 | } |
bdbohn | 0:d14cf1e75576 | 304 | else |
bdbohn | 0:d14cf1e75576 | 305 | { |
bdbohn | 0:d14cf1e75576 | 306 | OE = 1; |
bdbohn | 0:d14cf1e75576 | 307 | } |
bdbohn | 0:d14cf1e75576 | 308 | break; |
bdbohn | 0:d14cf1e75576 | 309 | |
bdbohn | 0:d14cf1e75576 | 310 | case RUN: |
bdbohn | 0:d14cf1e75576 | 311 | if(runStopChange != 0) |
bdbohn | 0:d14cf1e75576 | 312 | { |
bdbohn | 0:d14cf1e75576 | 313 | OE = 0; |
bdbohn | 0:d14cf1e75576 | 314 | runStopChange = 0; |
bdbohn | 0:d14cf1e75576 | 315 | } |
bdbohn | 0:d14cf1e75576 | 316 | break; |
bdbohn | 0:d14cf1e75576 | 317 | |
bdbohn | 0:d14cf1e75576 | 318 | case RESET: |
bdbohn | 0:d14cf1e75576 | 319 | OE = 1; |
bdbohn | 0:d14cf1e75576 | 320 | runStopChange = false; |
bdbohn | 0:d14cf1e75576 | 321 | motorState = STOP; |
bdbohn | 0:d14cf1e75576 | 322 | break; |
bdbohn | 0:d14cf1e75576 | 323 | |
bdbohn | 0:d14cf1e75576 | 324 | case RAMP: |
bdbohn | 0:d14cf1e75576 | 325 | if(acceleration_start == 1) |
bdbohn | 0:d14cf1e75576 | 326 | { |
bdbohn | 0:d14cf1e75576 | 327 | OE = 0; |
bdbohn | 0:d14cf1e75576 | 328 | acceleration_a(); |
bdbohn | 0:d14cf1e75576 | 329 | acceleration_start = 0; |
bdbohn | 0:d14cf1e75576 | 330 | } |
bdbohn | 0:d14cf1e75576 | 331 | if(accel_wait_flag == 0) |
bdbohn | 0:d14cf1e75576 | 332 | { |
bdbohn | 0:d14cf1e75576 | 333 | acceleration_a(); |
bdbohn | 0:d14cf1e75576 | 334 | accel_wait_flag = 1; |
bdbohn | 0:d14cf1e75576 | 335 | accel_ramp.attach_us(&clear_accel_wait_flag, next_interval_us); |
bdbohn | 0:d14cf1e75576 | 336 | } |
bdbohn | 0:d14cf1e75576 | 337 | |
bdbohn | 0:d14cf1e75576 | 338 | break; |
bdbohn | 0:d14cf1e75576 | 339 | |
bdbohn | 0:d14cf1e75576 | 340 | }// end switch motorState |
bdbohn | 0:d14cf1e75576 | 341 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
bdbohn | 0:d14cf1e75576 | 342 | |
bdbohn | 0:d14cf1e75576 | 343 | |
bdbohn | 0:d14cf1e75576 | 344 | }//end while |
bdbohn | 0:d14cf1e75576 | 345 | }//end main |
bdbohn | 0:d14cf1e75576 | 346 | |
bdbohn | 0:d14cf1e75576 | 347 | |
bdbohn | 0:d14cf1e75576 | 348 | |
bdbohn | 0:d14cf1e75576 | 349 | |
bdbohn | 0:d14cf1e75576 | 350 | |
bdbohn | 0:d14cf1e75576 | 351 | |
bdbohn | 0:d14cf1e75576 | 352 | |
bdbohn | 0:d14cf1e75576 | 353 | |
bdbohn | 0:d14cf1e75576 | 354 | |
bdbohn | 0:d14cf1e75576 | 355 | |
bdbohn | 0:d14cf1e75576 | 356 | //state machine to advance to next step |
bdbohn | 0:d14cf1e75576 | 357 | //called each time |
bdbohn | 0:d14cf1e75576 | 358 | void adv_state_mach_half() |
bdbohn | 0:d14cf1e75576 | 359 | { |
bdbohn | 0:d14cf1e75576 | 360 | |
bdbohn | 0:d14cf1e75576 | 361 | READY = 1; |
bdbohn | 0:d14cf1e75576 | 362 | if(OE == 0) |
bdbohn | 0:d14cf1e75576 | 363 | { |
bdbohn | 0:d14cf1e75576 | 364 | |
bdbohn | 0:d14cf1e75576 | 365 | if(dir == 1) |
bdbohn | 0:d14cf1e75576 | 366 | { |
bdbohn | 0:d14cf1e75576 | 367 | switch(stepState) |
bdbohn | 0:d14cf1e75576 | 368 | { |
bdbohn | 0:d14cf1e75576 | 369 | case STATE1: |
bdbohn | 0:d14cf1e75576 | 370 | IN1A = 1; //coil A +, coil B + |
bdbohn | 0:d14cf1e75576 | 371 | IN1B = 0; |
bdbohn | 0:d14cf1e75576 | 372 | IN2A = 1; |
bdbohn | 0:d14cf1e75576 | 373 | IN2B = 0; |
bdbohn | 0:d14cf1e75576 | 374 | stepState = 2; |
bdbohn | 0:d14cf1e75576 | 375 | break; |
bdbohn | 0:d14cf1e75576 | 376 | |
bdbohn | 0:d14cf1e75576 | 377 | case STATE2: |
bdbohn | 0:d14cf1e75576 | 378 | IN1A = 0; //coil A 0, coil B + |
bdbohn | 0:d14cf1e75576 | 379 | IN1B = 0; |
bdbohn | 0:d14cf1e75576 | 380 | IN2A = 1; |
bdbohn | 0:d14cf1e75576 | 381 | IN2B = 0; |
bdbohn | 0:d14cf1e75576 | 382 | stepState = 3; |
bdbohn | 0:d14cf1e75576 | 383 | break; |
bdbohn | 0:d14cf1e75576 | 384 | |
bdbohn | 0:d14cf1e75576 | 385 | case STATE3: |
bdbohn | 0:d14cf1e75576 | 386 | IN1A = 0; //coil A -, coil B + |
bdbohn | 0:d14cf1e75576 | 387 | IN1B = 1; |
bdbohn | 0:d14cf1e75576 | 388 | IN2A = 1; |
bdbohn | 0:d14cf1e75576 | 389 | IN2B = 0; |
bdbohn | 0:d14cf1e75576 | 390 | stepState = 4; |
bdbohn | 0:d14cf1e75576 | 391 | break; |
bdbohn | 0:d14cf1e75576 | 392 | |
bdbohn | 0:d14cf1e75576 | 393 | case STATE4: |
bdbohn | 0:d14cf1e75576 | 394 | IN1A = 0; //coil A -, coil B 0 |
bdbohn | 0:d14cf1e75576 | 395 | IN1B = 1; |
bdbohn | 0:d14cf1e75576 | 396 | IN2A = 0; |
bdbohn | 0:d14cf1e75576 | 397 | IN2B = 0; |
bdbohn | 0:d14cf1e75576 | 398 | stepState = 5; |
bdbohn | 0:d14cf1e75576 | 399 | break; |
bdbohn | 0:d14cf1e75576 | 400 | |
bdbohn | 0:d14cf1e75576 | 401 | case STATE5: |
bdbohn | 0:d14cf1e75576 | 402 | IN1A = 0; //coil A -, coil B - |
bdbohn | 0:d14cf1e75576 | 403 | IN1B = 1; |
bdbohn | 0:d14cf1e75576 | 404 | IN2A = 0; |
bdbohn | 0:d14cf1e75576 | 405 | IN2B = 1; |
bdbohn | 0:d14cf1e75576 | 406 | stepState = 6; |
bdbohn | 0:d14cf1e75576 | 407 | break; |
bdbohn | 0:d14cf1e75576 | 408 | |
bdbohn | 0:d14cf1e75576 | 409 | case STATE6: |
bdbohn | 0:d14cf1e75576 | 410 | IN1A = 0; //coil A 0, coil B - |
bdbohn | 0:d14cf1e75576 | 411 | IN1B = 0; |
bdbohn | 0:d14cf1e75576 | 412 | IN2A = 0; |
bdbohn | 0:d14cf1e75576 | 413 | IN2B = 1; |
bdbohn | 0:d14cf1e75576 | 414 | stepState = 7; |
bdbohn | 0:d14cf1e75576 | 415 | break; |
bdbohn | 0:d14cf1e75576 | 416 | |
bdbohn | 0:d14cf1e75576 | 417 | case STATE7: |
bdbohn | 0:d14cf1e75576 | 418 | IN1A = 1; //coil A +, coil B - |
bdbohn | 0:d14cf1e75576 | 419 | IN1B = 0; |
bdbohn | 0:d14cf1e75576 | 420 | IN2A = 0; |
bdbohn | 0:d14cf1e75576 | 421 | IN2B = 1; |
bdbohn | 0:d14cf1e75576 | 422 | stepState = 8; |
bdbohn | 0:d14cf1e75576 | 423 | break; |
bdbohn | 0:d14cf1e75576 | 424 | |
bdbohn | 0:d14cf1e75576 | 425 | case STATE8: |
bdbohn | 0:d14cf1e75576 | 426 | IN1A = 1; //coil A +, coil B 0 |
bdbohn | 0:d14cf1e75576 | 427 | IN1B = 0; |
bdbohn | 0:d14cf1e75576 | 428 | IN2A = 0; |
bdbohn | 0:d14cf1e75576 | 429 | IN2B = 0; |
bdbohn | 0:d14cf1e75576 | 430 | stepState = 1; |
bdbohn | 0:d14cf1e75576 | 431 | break; |
bdbohn | 0:d14cf1e75576 | 432 | |
bdbohn | 0:d14cf1e75576 | 433 | default: |
bdbohn | 0:d14cf1e75576 | 434 | IN1A = 0; |
bdbohn | 0:d14cf1e75576 | 435 | IN1B = 0; |
bdbohn | 0:d14cf1e75576 | 436 | IN2A = 0; |
bdbohn | 0:d14cf1e75576 | 437 | IN2B = 0; |
bdbohn | 0:d14cf1e75576 | 438 | stepState = 1; |
bdbohn | 0:d14cf1e75576 | 439 | break; |
bdbohn | 0:d14cf1e75576 | 440 | |
bdbohn | 0:d14cf1e75576 | 441 | |
bdbohn | 0:d14cf1e75576 | 442 | } |
bdbohn | 0:d14cf1e75576 | 443 | } |
bdbohn | 0:d14cf1e75576 | 444 | else |
bdbohn | 0:d14cf1e75576 | 445 | { |
bdbohn | 0:d14cf1e75576 | 446 | switch(stepState) |
bdbohn | 0:d14cf1e75576 | 447 | { |
bdbohn | 0:d14cf1e75576 | 448 | case STATE1: |
bdbohn | 0:d14cf1e75576 | 449 | IN1A = 1; //coil A +, coil B 0 |
bdbohn | 0:d14cf1e75576 | 450 | IN1B = 0; |
bdbohn | 0:d14cf1e75576 | 451 | IN2A = 0; |
bdbohn | 0:d14cf1e75576 | 452 | IN2B = 0; |
bdbohn | 0:d14cf1e75576 | 453 | stepState = 2; |
bdbohn | 0:d14cf1e75576 | 454 | break; |
bdbohn | 0:d14cf1e75576 | 455 | |
bdbohn | 0:d14cf1e75576 | 456 | case STATE2: |
bdbohn | 0:d14cf1e75576 | 457 | IN1A = 1; //coil A +, coil B - |
bdbohn | 0:d14cf1e75576 | 458 | IN1B = 0; |
bdbohn | 0:d14cf1e75576 | 459 | IN2A = 0; |
bdbohn | 0:d14cf1e75576 | 460 | IN2B = 1; |
bdbohn | 0:d14cf1e75576 | 461 | stepState = 3; |
bdbohn | 0:d14cf1e75576 | 462 | break; |
bdbohn | 0:d14cf1e75576 | 463 | |
bdbohn | 0:d14cf1e75576 | 464 | case STATE3: |
bdbohn | 0:d14cf1e75576 | 465 | IN1A = 0; //coil A 0 , coil B - |
bdbohn | 0:d14cf1e75576 | 466 | IN1B = 0; |
bdbohn | 0:d14cf1e75576 | 467 | IN2A = 0; |
bdbohn | 0:d14cf1e75576 | 468 | IN2B = 1; |
bdbohn | 0:d14cf1e75576 | 469 | stepState = 4; |
bdbohn | 0:d14cf1e75576 | 470 | break; |
bdbohn | 0:d14cf1e75576 | 471 | |
bdbohn | 0:d14cf1e75576 | 472 | case STATE4: |
bdbohn | 0:d14cf1e75576 | 473 | IN1A = 0; //coil A -, coil B - |
bdbohn | 0:d14cf1e75576 | 474 | IN1B = 1; |
bdbohn | 0:d14cf1e75576 | 475 | IN2A = 0; |
bdbohn | 0:d14cf1e75576 | 476 | IN2B = 1; |
bdbohn | 0:d14cf1e75576 | 477 | stepState = 5; |
bdbohn | 0:d14cf1e75576 | 478 | break; |
bdbohn | 0:d14cf1e75576 | 479 | |
bdbohn | 0:d14cf1e75576 | 480 | case STATE5: |
bdbohn | 0:d14cf1e75576 | 481 | IN1A = 0; //coil A -, coil B 0 |
bdbohn | 0:d14cf1e75576 | 482 | IN1B = 1; |
bdbohn | 0:d14cf1e75576 | 483 | IN2A = 0; |
bdbohn | 0:d14cf1e75576 | 484 | IN2B = 0; |
bdbohn | 0:d14cf1e75576 | 485 | stepState = 6; |
bdbohn | 0:d14cf1e75576 | 486 | break; |
bdbohn | 0:d14cf1e75576 | 487 | |
bdbohn | 0:d14cf1e75576 | 488 | case STATE6: |
bdbohn | 0:d14cf1e75576 | 489 | IN1A = 0; //coil A -, coil B + |
bdbohn | 0:d14cf1e75576 | 490 | IN1B = 1; |
bdbohn | 0:d14cf1e75576 | 491 | IN2A = 1; |
bdbohn | 0:d14cf1e75576 | 492 | IN2B = 0; |
bdbohn | 0:d14cf1e75576 | 493 | stepState = 7; |
bdbohn | 0:d14cf1e75576 | 494 | break; |
bdbohn | 0:d14cf1e75576 | 495 | |
bdbohn | 0:d14cf1e75576 | 496 | case STATE7: |
bdbohn | 0:d14cf1e75576 | 497 | IN1A = 0; //coil A 0, coil B + |
bdbohn | 0:d14cf1e75576 | 498 | IN1B = 0; |
bdbohn | 0:d14cf1e75576 | 499 | IN2A = 1; |
bdbohn | 0:d14cf1e75576 | 500 | IN2B = 0; |
bdbohn | 0:d14cf1e75576 | 501 | stepState = 8; |
bdbohn | 0:d14cf1e75576 | 502 | break; |
bdbohn | 0:d14cf1e75576 | 503 | |
bdbohn | 0:d14cf1e75576 | 504 | case STATE8: |
bdbohn | 0:d14cf1e75576 | 505 | IN1A = 1; //coil A +, coil B + |
bdbohn | 0:d14cf1e75576 | 506 | IN1B = 0; |
bdbohn | 0:d14cf1e75576 | 507 | IN2A = 1; |
bdbohn | 0:d14cf1e75576 | 508 | IN2B = 0; |
bdbohn | 0:d14cf1e75576 | 509 | stepState = 1; |
bdbohn | 0:d14cf1e75576 | 510 | break; |
bdbohn | 0:d14cf1e75576 | 511 | |
bdbohn | 0:d14cf1e75576 | 512 | default: |
bdbohn | 0:d14cf1e75576 | 513 | IN1A = 0; |
bdbohn | 0:d14cf1e75576 | 514 | IN1B = 0; |
bdbohn | 0:d14cf1e75576 | 515 | IN2A = 0; |
bdbohn | 0:d14cf1e75576 | 516 | IN2B = 0; |
bdbohn | 0:d14cf1e75576 | 517 | stepState = 1; |
bdbohn | 0:d14cf1e75576 | 518 | break; |
bdbohn | 0:d14cf1e75576 | 519 | |
bdbohn | 0:d14cf1e75576 | 520 | |
bdbohn | 0:d14cf1e75576 | 521 | } |
bdbohn | 0:d14cf1e75576 | 522 | } |
bdbohn | 0:d14cf1e75576 | 523 | } |
bdbohn | 0:d14cf1e75576 | 524 | |
bdbohn | 0:d14cf1e75576 | 525 | } |
bdbohn | 0:d14cf1e75576 | 526 | |
bdbohn | 0:d14cf1e75576 | 527 | |
bdbohn | 0:d14cf1e75576 | 528 | void adv_state_mach_full() |
bdbohn | 0:d14cf1e75576 | 529 | { |
bdbohn | 0:d14cf1e75576 | 530 | if(OE == 0) |
bdbohn | 0:d14cf1e75576 | 531 | { |
bdbohn | 0:d14cf1e75576 | 532 | if(dir == 1) |
bdbohn | 0:d14cf1e75576 | 533 | { |
bdbohn | 0:d14cf1e75576 | 534 | switch(stepState) |
bdbohn | 0:d14cf1e75576 | 535 | { |
bdbohn | 0:d14cf1e75576 | 536 | case STATE1: |
bdbohn | 0:d14cf1e75576 | 537 | IN1A = 1; //coil A +, coil B + |
bdbohn | 0:d14cf1e75576 | 538 | IN1B = 0; |
bdbohn | 0:d14cf1e75576 | 539 | IN2A = 1; |
bdbohn | 0:d14cf1e75576 | 540 | IN2B = 0; |
bdbohn | 0:d14cf1e75576 | 541 | stepState = 2; |
bdbohn | 0:d14cf1e75576 | 542 | break; |
bdbohn | 0:d14cf1e75576 | 543 | |
bdbohn | 0:d14cf1e75576 | 544 | case STATE2: |
bdbohn | 0:d14cf1e75576 | 545 | IN1A = 1; //coil A +, coil B - |
bdbohn | 0:d14cf1e75576 | 546 | IN1B = 0; |
bdbohn | 0:d14cf1e75576 | 547 | IN2A = 0; |
bdbohn | 0:d14cf1e75576 | 548 | IN2B = 1; |
bdbohn | 0:d14cf1e75576 | 549 | stepState = 3; |
bdbohn | 0:d14cf1e75576 | 550 | break; |
bdbohn | 0:d14cf1e75576 | 551 | |
bdbohn | 0:d14cf1e75576 | 552 | case STATE3: |
bdbohn | 0:d14cf1e75576 | 553 | IN1A = 0; //coil A -, coil B - |
bdbohn | 0:d14cf1e75576 | 554 | IN1B = 1; |
bdbohn | 0:d14cf1e75576 | 555 | IN2A = 0; |
bdbohn | 0:d14cf1e75576 | 556 | IN2B = 1; |
bdbohn | 0:d14cf1e75576 | 557 | stepState = 4; |
bdbohn | 0:d14cf1e75576 | 558 | break; |
bdbohn | 0:d14cf1e75576 | 559 | |
bdbohn | 0:d14cf1e75576 | 560 | |
bdbohn | 0:d14cf1e75576 | 561 | case STATE4: |
bdbohn | 0:d14cf1e75576 | 562 | IN1A = 0; //coil A -, coil B + |
bdbohn | 0:d14cf1e75576 | 563 | IN1B = 1; |
bdbohn | 0:d14cf1e75576 | 564 | IN2A = 1; |
bdbohn | 0:d14cf1e75576 | 565 | IN2B = 0; |
bdbohn | 0:d14cf1e75576 | 566 | stepState = 1; |
bdbohn | 0:d14cf1e75576 | 567 | break; |
bdbohn | 0:d14cf1e75576 | 568 | |
bdbohn | 0:d14cf1e75576 | 569 | |
bdbohn | 0:d14cf1e75576 | 570 | |
bdbohn | 0:d14cf1e75576 | 571 | default: |
bdbohn | 0:d14cf1e75576 | 572 | IN1A = 0; |
bdbohn | 0:d14cf1e75576 | 573 | IN1B = 0; |
bdbohn | 0:d14cf1e75576 | 574 | IN2A = 0; |
bdbohn | 0:d14cf1e75576 | 575 | IN2B = 0; |
bdbohn | 0:d14cf1e75576 | 576 | stepState = 1; |
bdbohn | 0:d14cf1e75576 | 577 | break; |
bdbohn | 0:d14cf1e75576 | 578 | |
bdbohn | 0:d14cf1e75576 | 579 | |
bdbohn | 0:d14cf1e75576 | 580 | } |
bdbohn | 0:d14cf1e75576 | 581 | } |
bdbohn | 0:d14cf1e75576 | 582 | else |
bdbohn | 0:d14cf1e75576 | 583 | { |
bdbohn | 0:d14cf1e75576 | 584 | switch(stepState) |
bdbohn | 0:d14cf1e75576 | 585 | { |
bdbohn | 0:d14cf1e75576 | 586 | case STATE1: |
bdbohn | 0:d14cf1e75576 | 587 | IN1A = 0; //coil A -, coil B + |
bdbohn | 0:d14cf1e75576 | 588 | IN1B = 1; |
bdbohn | 0:d14cf1e75576 | 589 | IN2A = 1; |
bdbohn | 0:d14cf1e75576 | 590 | IN2B = 0; |
bdbohn | 0:d14cf1e75576 | 591 | stepState = 2; |
bdbohn | 0:d14cf1e75576 | 592 | break; |
bdbohn | 0:d14cf1e75576 | 593 | |
bdbohn | 0:d14cf1e75576 | 594 | case STATE2: |
bdbohn | 0:d14cf1e75576 | 595 | IN1A = 0; //coil A -, coil B - |
bdbohn | 0:d14cf1e75576 | 596 | IN1B = 1; |
bdbohn | 0:d14cf1e75576 | 597 | IN2A = 0; |
bdbohn | 0:d14cf1e75576 | 598 | IN2B = 1; |
bdbohn | 0:d14cf1e75576 | 599 | stepState = 3; |
bdbohn | 0:d14cf1e75576 | 600 | break; |
bdbohn | 0:d14cf1e75576 | 601 | |
bdbohn | 0:d14cf1e75576 | 602 | case STATE3: |
bdbohn | 0:d14cf1e75576 | 603 | IN1A = 1; //coil A +, coil B - |
bdbohn | 0:d14cf1e75576 | 604 | IN1B = 0; |
bdbohn | 0:d14cf1e75576 | 605 | IN2A = 0; |
bdbohn | 0:d14cf1e75576 | 606 | IN2B = 1; |
bdbohn | 0:d14cf1e75576 | 607 | stepState = 4; |
bdbohn | 0:d14cf1e75576 | 608 | break; |
bdbohn | 0:d14cf1e75576 | 609 | |
bdbohn | 0:d14cf1e75576 | 610 | case STATE4: |
bdbohn | 0:d14cf1e75576 | 611 | IN1A = 1; //coil A +, coil B + |
bdbohn | 0:d14cf1e75576 | 612 | IN1B = 0; |
bdbohn | 0:d14cf1e75576 | 613 | IN2A = 1; |
bdbohn | 0:d14cf1e75576 | 614 | IN2B = 0; |
bdbohn | 0:d14cf1e75576 | 615 | stepState = 1; |
bdbohn | 0:d14cf1e75576 | 616 | break; |
bdbohn | 0:d14cf1e75576 | 617 | |
bdbohn | 0:d14cf1e75576 | 618 | default: |
bdbohn | 0:d14cf1e75576 | 619 | IN1A = 0; |
bdbohn | 0:d14cf1e75576 | 620 | IN1B = 0; |
bdbohn | 0:d14cf1e75576 | 621 | IN2A = 0; |
bdbohn | 0:d14cf1e75576 | 622 | IN2B = 0; |
bdbohn | 0:d14cf1e75576 | 623 | stepState = 1; |
bdbohn | 0:d14cf1e75576 | 624 | break; |
bdbohn | 0:d14cf1e75576 | 625 | |
bdbohn | 0:d14cf1e75576 | 626 | |
bdbohn | 0:d14cf1e75576 | 627 | } //end switch |
bdbohn | 0:d14cf1e75576 | 628 | } //end else |
bdbohn | 0:d14cf1e75576 | 629 | } |
bdbohn | 0:d14cf1e75576 | 630 | } |
bdbohn | 0:d14cf1e75576 | 631 | ///////////////////////////////////////////////////////////////////////////////// |
bdbohn | 0:d14cf1e75576 | 632 | |
bdbohn | 0:d14cf1e75576 | 633 | |
bdbohn | 0:d14cf1e75576 | 634 | void clear_accel_wait_flag(void) |
bdbohn | 0:d14cf1e75576 | 635 | { |
bdbohn | 0:d14cf1e75576 | 636 | accel_wait_flag = 0; |
bdbohn | 0:d14cf1e75576 | 637 | } |
bdbohn | 0:d14cf1e75576 | 638 | |
bdbohn | 0:d14cf1e75576 | 639 | |
bdbohn | 0:d14cf1e75576 | 640 | |
bdbohn | 0:d14cf1e75576 | 641 | |
bdbohn | 0:d14cf1e75576 | 642 | void acceleration_a(void) |
bdbohn | 0:d14cf1e75576 | 643 | { |
bdbohn | 0:d14cf1e75576 | 644 | |
bdbohn | 0:d14cf1e75576 | 645 | rampCount ++; |
bdbohn | 0:d14cf1e75576 | 646 | if(rampCount <= numRampSteps) |
bdbohn | 0:d14cf1e75576 | 647 | { |
bdbohn | 0:d14cf1e75576 | 648 | test_state_mach(); |
bdbohn | 0:d14cf1e75576 | 649 | next_interval_us = 1000000 * (1/float(rampCount * stepMultiplier)); |
bdbohn | 0:d14cf1e75576 | 650 | // accel_ramp.attach_us(&acceleration_b, next_interval_us);//speed); |
bdbohn | 0:d14cf1e75576 | 651 | } |
bdbohn | 0:d14cf1e75576 | 652 | else |
bdbohn | 0:d14cf1e75576 | 653 | { |
bdbohn | 0:d14cf1e75576 | 654 | rampCount = 0; |
bdbohn | 0:d14cf1e75576 | 655 | motorState = RUN; |
bdbohn | 0:d14cf1e75576 | 656 | advance_state.attach_us(&test_state_mach, stepInterval_us); |
bdbohn | 0:d14cf1e75576 | 657 | } |
bdbohn | 0:d14cf1e75576 | 658 | |
bdbohn | 0:d14cf1e75576 | 659 | } |
bdbohn | 0:d14cf1e75576 | 660 | ///////////////////////////////////////////////////////////////////////////////// |
bdbohn | 0:d14cf1e75576 | 661 | void acceleration_b(void) |
bdbohn | 0:d14cf1e75576 | 662 | { |
bdbohn | 0:d14cf1e75576 | 663 | READY = 1; |
bdbohn | 0:d14cf1e75576 | 664 | rampCount ++; |
bdbohn | 0:d14cf1e75576 | 665 | if(rampCount <= 5000) //numRampSteps) |
bdbohn | 0:d14cf1e75576 | 666 | { |
bdbohn | 0:d14cf1e75576 | 667 | test_state_mach(); |
bdbohn | 0:d14cf1e75576 | 668 | next_interval_us = 1000000 * (1/(rampCount)); // * stepMultiplier)); |
bdbohn | 0:d14cf1e75576 | 669 | accel_ramp.attach_us(&acceleration_a, next_interval_us); |
bdbohn | 0:d14cf1e75576 | 670 | } |
bdbohn | 0:d14cf1e75576 | 671 | else |
bdbohn | 0:d14cf1e75576 | 672 | { |
bdbohn | 0:d14cf1e75576 | 673 | READY = 0; |
bdbohn | 0:d14cf1e75576 | 674 | rampCount = 0; |
bdbohn | 0:d14cf1e75576 | 675 | motorState = RUN; |
bdbohn | 0:d14cf1e75576 | 676 | advance_state.attach_us(&test_state_mach, stepInterval_us); |
bdbohn | 0:d14cf1e75576 | 677 | } |
bdbohn | 0:d14cf1e75576 | 678 | |
bdbohn | 0:d14cf1e75576 | 679 | } |
bdbohn | 0:d14cf1e75576 | 680 | ///////////////////////////////////////////////////////////////////////////////// |
bdbohn | 0:d14cf1e75576 | 681 | |
bdbohn | 0:d14cf1e75576 | 682 | void test_state_mach(void) |
bdbohn | 0:d14cf1e75576 | 683 | { |
bdbohn | 0:d14cf1e75576 | 684 | if(stepMode == QTR) |
bdbohn | 0:d14cf1e75576 | 685 | { |
bdbohn | 0:d14cf1e75576 | 686 | adv_state_mach_half(); |
bdbohn | 0:d14cf1e75576 | 687 | } |
bdbohn | 0:d14cf1e75576 | 688 | else |
bdbohn | 0:d14cf1e75576 | 689 | { |
bdbohn | 0:d14cf1e75576 | 690 | adv_state_mach_full(); |
bdbohn | 0:d14cf1e75576 | 691 | } |
bdbohn | 0:d14cf1e75576 | 692 | } |
bdbohn | 0:d14cf1e75576 | 693 | /////////////////////////////////////////////////////////////////////////////////// |
bdbohn | 0:d14cf1e75576 | 694 | |
bdbohn | 0:d14cf1e75576 | 695 | |
bdbohn | 0:d14cf1e75576 | 696 | void set_speed(float speed) |
bdbohn | 0:d14cf1e75576 | 697 | { |
bdbohn | 0:d14cf1e75576 | 698 | if(motorState == RUN) |
bdbohn | 0:d14cf1e75576 | 699 | { |
bdbohn | 0:d14cf1e75576 | 700 | if(stepMode == QTR) |
bdbohn | 0:d14cf1e75576 | 701 | { |
bdbohn | 0:d14cf1e75576 | 702 | numRampSteps = speed; |
bdbohn | 0:d14cf1e75576 | 703 | stepInterval_us = 1000000*(1.0/(speed * 8)); |
bdbohn | 0:d14cf1e75576 | 704 | advance_state.attach_us(&test_state_mach, stepInterval_us); |
bdbohn | 0:d14cf1e75576 | 705 | } |
bdbohn | 0:d14cf1e75576 | 706 | else |
bdbohn | 0:d14cf1e75576 | 707 | { |
bdbohn | 0:d14cf1e75576 | 708 | numRampSteps = speed; |
bdbohn | 0:d14cf1e75576 | 709 | stepInterval_us = 1000000*(1.0/(speed * 4)); |
bdbohn | 0:d14cf1e75576 | 710 | advance_state.attach_us(&test_state_mach, stepInterval_us); |
bdbohn | 0:d14cf1e75576 | 711 | } |
bdbohn | 0:d14cf1e75576 | 712 | } |
bdbohn | 0:d14cf1e75576 | 713 | else //not in run mode, save the change |
bdbohn | 0:d14cf1e75576 | 714 | { |
bdbohn | 0:d14cf1e75576 | 715 | if(stepMode == QTR) |
bdbohn | 0:d14cf1e75576 | 716 | { |
bdbohn | 0:d14cf1e75576 | 717 | numRampSteps = speed; |
bdbohn | 0:d14cf1e75576 | 718 | stepInterval_us = 1000000*(1.0/(speed * 8)); |
bdbohn | 0:d14cf1e75576 | 719 | } |
bdbohn | 0:d14cf1e75576 | 720 | else |
bdbohn | 0:d14cf1e75576 | 721 | { |
bdbohn | 0:d14cf1e75576 | 722 | stepInterval_us = 1000000*(1.0/(speed * 4)); |
bdbohn | 0:d14cf1e75576 | 723 | numRampSteps = speed; |
bdbohn | 0:d14cf1e75576 | 724 | } |
bdbohn | 0:d14cf1e75576 | 725 | } |
bdbohn | 0:d14cf1e75576 | 726 | } |