Moorthy Muthukrishnan
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 
NXP
Diff: main.cpp
- Revision:
- 1:48b0212c91e9
- Parent:
- 0:d14cf1e75576
--- a/main.cpp Fri Nov 14 21:03:06 2014 +0000
+++ b/main.cpp Tue Sep 20 09:22:36 2016 +0000
@@ -1,726 +1,402 @@
+/*
+ Written by Moorthy Muthukrishnan
+ KL25Z program for driving bipolar stepper motor
+ Stepper motor model 17HA0403-32N
+ Driver board using L293D H-bridge driver IC
+ Note:
+ ULN2003-based drivers work well for unipolar stepper motors
+ but did not work with the bipoar stepper motors.
+
+ IN1A = Coil A+ IN1B = Coil B+
+ Y = coil 3 Orange = coil 4
+ Red = common
+
+ A Variable named mode is used to select the mode of operation of the stepper motor.
+ wave drive mode = 0
+ full step mode = 1
+ half step mode = 2
+
+ A variable named dir is used to define the direction of rotation
+ direction dir = 0 anticlockwise
+ direction dir = 1 clockwise
+*/
//Libraries needed
#include "mbed.h"
-#include "USBHID.h"
-// We declare a USBHID device.
-// HID In/Out Reports are 64 Bytes long
-// Vendor ID (VID): 0x15A2
-// Product ID (PID): 0x0138
-// Serial Number: 0x0001
-USBHID hid(64, 64, 0x15A2, 0x0138, 0x0001, true);
-
-//storage for send and receive data
-HID_REPORT send_report;
-HID_REPORT recv_report;
-
-//Stepper Motor States
-#define STATE1 1
-#define STATE2 2
-#define STATE3 3
-#define STATE4 4
-#define STATE5 5
-#define STATE6 6
-#define STATE7 7
-#define STATE8 8
-
-// USB COMMANDS
-// These are sent from the PC
-#define WRITE_LED 0x20
-#define WRITE_OUTPUT_EN 0x30
-#define WRITE_STEPS_PER_SEC 0x40
-#define WRITE_RUN_STOP 0x70
-#define WRITE_DIRECTION 0x71
-#define WRITE_ACCEL 0x80
-#define WRITE_RESET 0xA0
-#define WRITE_STEPMODE 0xB0
-
-
-// MOTOR STATES
-#define STOP 0x00
-#define RUN 0x02
-#define RAMP 0x03
-#define RESET 0x05
-
-// LED CONSTANTS
-#define LEDS_OFF 0x00
-#define RED 0x01
-#define GREEN 0x02
-#define BLUE 0x03
-#define READY_LED 0x04
-
-// LOGICAL CONSTANTS
-#define OFF 0x00
-#define ON 0x01
-
-//step modes
-#define QTR 0x01
-#define HALF 0x02
-
-//FRDM-KL25Z LEDs
-DigitalOut red_led(LED1);
-DigitalOut green_led(LED2);
-DigitalOut blue_led(LED3);
+#define DLY 0.1 // DLY 1s
//Input pins on Eval Board
-DigitalOut IN1A(PTD4); // Pin IN1A input to EVAL board (FRDM PIN Name)
-DigitalOut IN1B(PTA12); // Pin IN1B input to EVAL board (FRDM PIN Name)
-DigitalOut IN2A(PTA4); // Pin IN2A input to EVAL board (FRDM PIN Name)
-DigitalOut IN2B(PTA5); // Pin IN2B input to EVAL board (FRDM PIN Name)
+DigitalOut IN1A(PTB8); // Pin IN1A input to EVAL board (FRDM PIN Name)
+DigitalOut IN1B(PTB9); // Pin IN1B input to EVAL board (FRDM PIN Name)
+DigitalOut IN2A(PTB10); // Pin IN2A input to EVAL board (FRDM PIN Name)
+DigitalOut IN2B(PTB11); // Pin IN2B input to EVAL board (FRDM PIN Name)
+Serial pc(USBTX,USBRX);
-//Green LED on Eval Board
-DigitalOut READY(PTC5); // Pin READY input to EVAL board (FRDM PIN Name)
-
-//These pins are defined differntly on different parts
-//OE for FRDM-17529 and FRDM-17533 Eval Boards
-//and PSAVE for FRDM-17C724 and FRDM-17531 Eval Boards
-//FRDM-34933 Eval Board does not have either of these pins
-DigitalOut OE(PTC7); // Pin OE input to MPC17533, MPC17529 (FRDM PIN Name)
+//variable
+static int mode = 0;
+static int dir = 0;
-//variables
-static int stepState = 1; //holds current step state of the step being applied to the stepper
-static int stepMode = 0; //This is either 1/2 or 1/4 step
-static int dir = 0; //rotational direction of stepper
-static int rampCount = 0; //counter value used during acceleration ramp-up
-static int initflag = 0; //used upon first entry into main at startup
-static int runstop = 0; //holds value of running or stopped of commanded value from PC
-static int motorState = 0; //holds state of stepper motor state machine (RUN, STOP, RESET, or RAMP)
-static int accel = 0; //holds the value of accceleration enabled from PC
-static float numRampSteps = 0; //calculated value that is based on the stepper speed after acceleration
-static float stepRate = 10;
-static int stepMultiplier = 1;
-static float next_interval_us = 1200;
-static float stepInterval_us = 1200;
-static bool acceleration_start = 0;
-static bool runStopChange = 0;
-static bool accel_wait_flag = 0;
-
-//static int mode = 0;
+void wave_anticlockwise(void);
+void fullstep_anticlockwise(void);
+void halfstep_anticlockwise(void);
-void test_state_mach(void);
-void adv_state_mach_half(void);
-void adv_state_mach_full(void);
-void set_speed(float stepRate);
-void acceleration_a(void);
-void acceleration_b(void);
-void set_speed_with_ramp(void);
-void null_function(void);
-void clear_accel_wait_flag(void);
+void wave_clockwise(void);
+void fullstep_clockwise(void);
+void halfstep_clockwise(void);
-
+void motor_idle(void);
-//
-//Create a Timeout function. Used for ramping up the speed at startup
-Timeout accel_ramp;
-
-//Create a Ticker function. Called during normal runtime to advance the stepper motor
-Ticker advance_state;
-
-
int main()
{
- //set up storage for incoming/outgoing
- send_report.length = 64;
- recv_report.length = 64;
+
+ // enter mode in the following line
+ // 0 for wave drive, 1 for full step, 2 for half step
+
+ mode = 0;
- red_led = 1; // Red LED = OFF
- green_led = 1; // Green LED = OFF
- blue_led = 0; // Blue LED = ON
+ //enter dir = 0 for anticlockwise rotation, dir = 1 for clockwise rotation
+ dir = 1;
- READY = 0;
+ pc.printf("\n\r");
+ pc.printf("****** Stepper Motor starting ******* \n\r");
+ pc.printf("Mode = %d Direction = %d \n\r", mode, dir);
- motorState = RESET;
-
- while(1)
+ while (true)
{
- //try to read a msg
- if(hid.readNB(&recv_report))
+ if (dir == 0)
{
- if(initflag == true)
+ pc.printf("Motor in anticlockwise rotation\n\r");
+ // wave drive
+ if (mode == 0)
+ {
+ wave_anticlockwise();
+ }
+
+ // full step
+ else if (mode == 1)
+ {
+ fullstep_anticlockwise();
+ }
+
+ // half step
+ else if (mode == 2)
+ {
+ halfstep_anticlockwise();
+ }
+ else
{
- initflag = false;
- blue_led = 1; //turn off blue LED
- READY = 0;
+ pc.printf("Invalid mode, Motor idle \n\r");
+ motor_idle();
+ }
+ }
+
+ else if (dir == 1)
+ {
+ pc.printf("Motor in clockwise rotation \n\r");
+ // wave drive
+ if (mode == 0)
+ {
+ wave_clockwise();
}
- switch(recv_report.data[0]) //byte 0 of recv_report.data is command
+
+ // full step
+ else if (mode == 1)
+ {
+ fullstep_clockwise();
+ }
+
+ // half step
+ else if (mode == 2)
+ {
+ halfstep_clockwise();
+ }
+
+ else
{
-//-----------------------------------------------------------------------------------------------------------------
-// COMMAND PARSER
-//-----------------------------------------------------------------------------------------------------------------
-////////
- case WRITE_LED:
- switch(recv_report.data[1])
- {
- case LEDS_OFF:
- red_led = 1;
- green_led = 1;
- blue_led = 1;
- break;
- case RED:
- if(recv_report.data[2] == 1){red_led = 0;} else {red_led = 1;}
- break;
- case GREEN:
- if(recv_report.data[2] == 1){green_led = 0;} else {green_led = 1;}
- break;
- case BLUE:
- if(recv_report.data[2] == 1){blue_led = 0;} else {blue_led = 1;}
- break;
- default:
- break;
- }// End recv report data[1]
- break;
-////////end case WRITE_LED
+ pc.printf("Invalid mode, Motor idle \n\r");
+ motor_idle();
+ }
+
+ }
+
+ else
+ {
+ pc.printf("Invalid direction, Motor idle \n\r");
+ motor_idle();
+ }
+
+ }
+}
-////////
- case WRITE_STEPS_PER_SEC:
+// function for wave drive in anticlockwise direction
+void wave_anticlockwise()
+{
+ pc.printf("Motor in wave drive \n\r");
+
+ //State8
+ IN1A = 1; //coil A +, coil B 0
+ IN1B = 0;
+ IN2A = 0;
+ IN2B = 0;
+ wait(DLY);
+
+ //State6
+ IN1A = 0; //coil A 0, coil B -
+ IN1B = 0;
+ IN2A = 0;
+ IN2B = 1;
+ wait(DLY);
+
+ //State4
+ IN1A = 0; //coil A -, coil B 0
+ IN1B = 1;
+ IN2A = 0;
+ IN2B = 0;
+ wait(DLY);
+
+ //State2
+ IN1A = 0; //coil A 0, coil B +
+ IN1B = 0;
+ IN2A = 1;
+ IN2B = 0;
+ wait(DLY);
+}
+
+// function for full step in anticlockwise direction
+void fullstep_anticlockwise()
+{
+ pc.printf("Motor in full step \n\r");
+
+ //State7
+ IN1A = 1; //coil A +, coil B -
+ IN1B = 0;
+ IN2A = 0;
+ IN2B = 1;
+ wait(DLY);
+
+ //State5
+ IN1A = 0; //coil A -, coil B -
+ IN1B = 1;
+ IN2A = 0;
+ IN2B = 1;
+ wait(DLY);
+
+ //State3
+ IN1A = 0; //coil A -, coil B +
+ IN1B = 1;
+ IN2A = 1;
+ IN2B = 0;
+ wait(DLY);
+
+ //State1
+ IN1A = 1; //coil A +, coil B +
+ IN1B = 0;
+ IN2A = 1;
+ IN2B = 0;
+ wait(DLY);
+
+}
+
+// function for half step in anticlockwise direction
+void halfstep_anticlockwise()
+{
+ pc.printf("Motor in half step \n\r");
- stepRate = recv_report.data[1];
- set_speed(stepRate);
- break;
-////////
- case WRITE_RUN_STOP:
- if(recv_report.data[1] == 1)
- {
- if(runstop != 1)
- {
- red_led = 1;
- blue_led = 1;
- green_led = 0;
- runstop = 1;
- runStopChange = 1;
-
- if(accel == 1)
- {
- motorState = RAMP;
- next_interval_us = stepMultiplier;
- acceleration_start = 1;
- }
- else
- {
- motorState = RUN;
- set_speed(stepRate);
- }
- }
-
- }
- else
- {
- if(runstop != 0)
- {
- runstop = 0;
- runStopChange = 1;
- motorState = STOP;
- red_led = 0;
- green_led = 1;
- blue_led = 1;
- }
- }//end if(recv_report.data[1] == 1)
- break;
-////////
- case WRITE_DIRECTION:
-
- if(recv_report.data[1] == 1)
- {
- dir = 1;
-
- }
- else
- {
- dir = 0;
-
- }
- break;
-////////
- case WRITE_ACCEL:
- if(recv_report.data[1] == 1)
- {
- accel = 1;
- }
- else
- {
- accel = 0;
- }
- break;
-////////
- case WRITE_STEPMODE:
- if(recv_report.data[1] == QTR)
- {
- stepMode = QTR;
- stepMultiplier = 8;
-
- }
- else
- {
- stepMode = HALF;
- stepMultiplier = 4;
+ //State8
+ IN1A = 1; //coil A +, coil B 0
+ IN1B = 0;
+ IN2A = 0;
+ IN2B = 0;
+ wait(DLY);
+
+ //State7
+ IN1A = 1; //coil A +, coil B -
+ IN1B = 0;
+ IN2A = 0;
+ IN2B = 1;
+ wait(DLY);
+
+ //State6
+ IN1A = 0; //coil A 0, coil B -
+ IN1B = 0;
+ IN2A = 0;
+ IN2B = 1;
+ wait(DLY);
+
+ //State5
+ IN1A = 0; //coil A -, coil B -
+ IN1B = 1;
+ IN2A = 0;
+ IN2B = 1;
+ wait(DLY);
+
+ //State4
+ IN1A = 0; //coil A -, coil B 0
+ IN1B = 1;
+ IN2A = 0;
+ IN2B = 0;
+ wait(DLY);
+
+ //State3
+ IN1A = 0; //coil A -, coil B +
+ IN1B = 1;
+ IN2A = 1;
+ IN2B = 0;
+ wait(DLY);
+
+ //State2
+ IN1A = 0; //coil A 0, coil B +
+ IN1B = 0;
+ IN2A = 1;
+ IN2B = 0;
+ wait(DLY);
+
+ //State1
+ IN1A = 1; //coil A +, coil B +
+ IN1B = 0;
+ IN2A = 1;
+ IN2B = 0;
+ wait(DLY);
+}
+
+// function for wave drive in clockwise direction
+void wave_clockwise()
+{
+ pc.printf("Motor in wave drive \n\r");
+
+ //State2
+ IN1A = 0; //coil A 0, coil B +
+ IN1B = 0;
+ IN2A = 1;
+ IN2B = 0;
+ wait(DLY);
+
+ //State4
+ IN1A = 0; //coil A -, coil B 0
+ IN1B = 1;
+ IN2A = 0;
+ IN2B = 0;
+ wait(DLY);
+
+ //State6
+ IN1A = 0; //coil A 0, coil B -
+ IN1B = 0;
+ IN2A = 0;
+ IN2B = 1;
+ wait(DLY);
+
+ //State8
+ IN1A = 1; //coil A +, coil B 0
+ IN1B = 0;
+ IN2A = 0;
+ IN2B = 0;
+ wait(DLY);
+}
+
+// function for full step in clockwise direction
+void fullstep_clockwise()
+{
+ pc.printf("Motor in full step \n\r");
- }
- break;
-////////
- default:
- break;
- }// End Switch recv report data[0]
-
+ //State1
+ IN1A = 1; //coil A +, coil B +
+ IN1B = 0;
+ IN2A = 1;
+ IN2B = 0;
+ wait(DLY);
+
+ //State3
+ IN1A = 0; //coil A -, coil B +
+ IN1B = 1;
+ IN2A = 1;
+ IN2B = 0;
+ wait(DLY);
+
+ //State5
+ IN1A = 0; //coil A -, coil B -
+ IN1B = 1;
+ IN2A = 0;
+ IN2B = 1;
+ wait(DLY);
+
+ //State7
+ IN1A = 1; //coil A +, coil B -
+ IN1B = 0;
+ IN2A = 0;
+ IN2B = 1;
+ wait(DLY);
+}
-//-----------------------------------------------------------------------------------------------------------------
-// end command parser
-//-----------------------------------------------------------------------------------------------------------------
-
- send_report.data[0] = recv_report.data[0]; // Echo Command
- send_report.data[1] = recv_report.data[1]; // Echo Subcommand 1
- send_report.data[2] = recv_report.data[2]; // Echo Subcommand 2
- send_report.data[3] = 0x00;
- send_report.data[4] = 0x00;
- send_report.data[5] = 0x00;
- send_report.data[6] = 0x00;
- send_report.data[7] = 0x00;
-
- //Send the report
- hid.send(&send_report);
- }// End If(hid.readNB(&recv_report))
-/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-//End of USB message handling
-/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-
-
-/************************************************************************************************************************
-// This is handling of Speed, Acceleration, Direction, and Start/Stop
-***********************************************************************************************************************/
-
-
-
-
-
+void halfstep_clockwise()
+{
+ pc.printf("Motor in half step \n\r");
- switch (motorState)
- {
- case STOP:
- if(runStopChange == 1)
- {
- runStopChange = 0;
- OE = 1;
- advance_state.detach();
- }
- else
- {
- OE = 1;
- }
- break;
-
- case RUN:
- if(runStopChange != 0)
- {
- OE = 0;
- runStopChange = 0;
- }
- break;
-
- case RESET:
- OE = 1;
- runStopChange = false;
- motorState = STOP;
- break;
-
- case RAMP:
- if(acceleration_start == 1)
- {
- OE = 0;
- acceleration_a();
- acceleration_start = 0;
- }
- if(accel_wait_flag == 0)
- {
- acceleration_a();
- accel_wait_flag = 1;
- accel_ramp.attach_us(&clear_accel_wait_flag, next_interval_us);
- }
+ //State1
+ IN1A = 1; //coil A +, coil B +
+ IN1B = 0;
+ IN2A = 1;
+ IN2B = 0;
+ wait(DLY);
+
+ //State2
+ IN1A = 0; //coil A 0, coil B +
+ IN1B = 0;
+ IN2A = 1;
+ IN2B = 0;
+ wait(DLY);
+
+ //State3
+ IN1A = 0; //coil A -, coil B +
+ IN1B = 1;
+ IN2A = 1;
+ IN2B = 0;
+ wait(DLY);
+
+ //State4
+ IN1A = 0; //coil A -, coil B 0
+ IN1B = 1;
+ IN2A = 0;
+ IN2B = 0;
+ wait(DLY);
+
+ //State5
+ IN1A = 0; //coil A -, coil B -
+ IN1B = 1;
+ IN2A = 0;
+ IN2B = 1;
+ wait(DLY);
+
+ //State6
+ IN1A = 0; //coil A 0, coil B -
+ IN1B = 0;
+ IN2A = 0;
+ IN2B = 1;
+ wait(DLY);
+
+ //State7
+ IN1A = 1; //coil A +, coil B -
+ IN1B = 0;
+ IN2A = 0;
+ IN2B = 1;
+ wait(DLY);
+
+ //State8
+ IN1A = 1; //coil A +, coil B 0
+ IN1B = 0;
+ IN2A = 0;
+ IN2B = 0;
+ wait(DLY);
+}
- break;
-
- }// end switch motorState
- ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-
-
- }//end while
-}//end main
-
+void motor_idle()
+{
+ //State0
+ IN1A = 0; //coil A 0, coil B 0
+ IN1B = 0;
+ IN2A = 0;
+ IN2B = 0;
+ wait(DLY);
+}
-
-
-
-
-
-
-
-//state machine to advance to next step
-//called each time
-void adv_state_mach_half()
-{
-
- READY = 1;
- if(OE == 0)
- {
-
- if(dir == 1)
- {
- switch(stepState)
- {
- case STATE1:
- IN1A = 1; //coil A +, coil B +
- IN1B = 0;
- IN2A = 1;
- IN2B = 0;
- stepState = 2;
- break;
-
- case STATE2:
- IN1A = 0; //coil A 0, coil B +
- IN1B = 0;
- IN2A = 1;
- IN2B = 0;
- stepState = 3;
- break;
-
- case STATE3:
- IN1A = 0; //coil A -, coil B +
- IN1B = 1;
- IN2A = 1;
- IN2B = 0;
- stepState = 4;
- break;
-
- case STATE4:
- IN1A = 0; //coil A -, coil B 0
- IN1B = 1;
- IN2A = 0;
- IN2B = 0;
- stepState = 5;
- break;
-
- case STATE5:
- IN1A = 0; //coil A -, coil B -
- IN1B = 1;
- IN2A = 0;
- IN2B = 1;
- stepState = 6;
- break;
-
- case STATE6:
- IN1A = 0; //coil A 0, coil B -
- IN1B = 0;
- IN2A = 0;
- IN2B = 1;
- stepState = 7;
- break;
-
- case STATE7:
- IN1A = 1; //coil A +, coil B -
- IN1B = 0;
- IN2A = 0;
- IN2B = 1;
- stepState = 8;
- break;
-
- case STATE8:
- IN1A = 1; //coil A +, coil B 0
- IN1B = 0;
- IN2A = 0;
- IN2B = 0;
- stepState = 1;
- break;
-
- default:
- IN1A = 0;
- IN1B = 0;
- IN2A = 0;
- IN2B = 0;
- stepState = 1;
- break;
-
-
- }
- }
- else
- {
- switch(stepState)
- {
- case STATE1:
- IN1A = 1; //coil A +, coil B 0
- IN1B = 0;
- IN2A = 0;
- IN2B = 0;
- stepState = 2;
- break;
-
- case STATE2:
- IN1A = 1; //coil A +, coil B -
- IN1B = 0;
- IN2A = 0;
- IN2B = 1;
- stepState = 3;
- break;
-
- case STATE3:
- IN1A = 0; //coil A 0 , coil B -
- IN1B = 0;
- IN2A = 0;
- IN2B = 1;
- stepState = 4;
- break;
-
- case STATE4:
- IN1A = 0; //coil A -, coil B -
- IN1B = 1;
- IN2A = 0;
- IN2B = 1;
- stepState = 5;
- break;
-
- case STATE5:
- IN1A = 0; //coil A -, coil B 0
- IN1B = 1;
- IN2A = 0;
- IN2B = 0;
- stepState = 6;
- break;
-
- case STATE6:
- IN1A = 0; //coil A -, coil B +
- IN1B = 1;
- IN2A = 1;
- IN2B = 0;
- stepState = 7;
- break;
-
- case STATE7:
- IN1A = 0; //coil A 0, coil B +
- IN1B = 0;
- IN2A = 1;
- IN2B = 0;
- stepState = 8;
- break;
-
- case STATE8:
- IN1A = 1; //coil A +, coil B +
- IN1B = 0;
- IN2A = 1;
- IN2B = 0;
- stepState = 1;
- break;
-
- default:
- IN1A = 0;
- IN1B = 0;
- IN2A = 0;
- IN2B = 0;
- stepState = 1;
- break;
-
-
- }
- }
- }
-
-}
-
-
-void adv_state_mach_full()
-{
- if(OE == 0)
- {
- if(dir == 1)
- {
- switch(stepState)
- {
- case STATE1:
- IN1A = 1; //coil A +, coil B +
- IN1B = 0;
- IN2A = 1;
- IN2B = 0;
- stepState = 2;
- break;
-
- case STATE2:
- IN1A = 1; //coil A +, coil B -
- IN1B = 0;
- IN2A = 0;
- IN2B = 1;
- stepState = 3;
- break;
-
- case STATE3:
- IN1A = 0; //coil A -, coil B -
- IN1B = 1;
- IN2A = 0;
- IN2B = 1;
- stepState = 4;
- break;
-
-
- case STATE4:
- IN1A = 0; //coil A -, coil B +
- IN1B = 1;
- IN2A = 1;
- IN2B = 0;
- stepState = 1;
- break;
-
-
-
- default:
- IN1A = 0;
- IN1B = 0;
- IN2A = 0;
- IN2B = 0;
- stepState = 1;
- break;
-
-
- }
- }
- else
- {
- switch(stepState)
- {
- case STATE1:
- IN1A = 0; //coil A -, coil B +
- IN1B = 1;
- IN2A = 1;
- IN2B = 0;
- stepState = 2;
- break;
-
- case STATE2:
- IN1A = 0; //coil A -, coil B -
- IN1B = 1;
- IN2A = 0;
- IN2B = 1;
- stepState = 3;
- break;
-
- case STATE3:
- IN1A = 1; //coil A +, coil B -
- IN1B = 0;
- IN2A = 0;
- IN2B = 1;
- stepState = 4;
- break;
-
- case STATE4:
- IN1A = 1; //coil A +, coil B +
- IN1B = 0;
- IN2A = 1;
- IN2B = 0;
- stepState = 1;
- break;
-
- default:
- IN1A = 0;
- IN1B = 0;
- IN2A = 0;
- IN2B = 0;
- stepState = 1;
- break;
-
-
- } //end switch
- } //end else
- }
-}
-/////////////////////////////////////////////////////////////////////////////////
-
-
-void clear_accel_wait_flag(void)
-{
- accel_wait_flag = 0;
-}
-
-
-
-
-void acceleration_a(void)
-{
-
- rampCount ++;
- if(rampCount <= numRampSteps)
- {
- test_state_mach();
- next_interval_us = 1000000 * (1/float(rampCount * stepMultiplier));
- // accel_ramp.attach_us(&acceleration_b, next_interval_us);//speed);
- }
- else
- {
- rampCount = 0;
- motorState = RUN;
- advance_state.attach_us(&test_state_mach, stepInterval_us);
- }
-
-}
-/////////////////////////////////////////////////////////////////////////////////
-void acceleration_b(void)
-{
- READY = 1;
- rampCount ++;
- if(rampCount <= 5000) //numRampSteps)
- {
- test_state_mach();
- next_interval_us = 1000000 * (1/(rampCount)); // * stepMultiplier));
- accel_ramp.attach_us(&acceleration_a, next_interval_us);
- }
- else
- {
- READY = 0;
- rampCount = 0;
- motorState = RUN;
- advance_state.attach_us(&test_state_mach, stepInterval_us);
- }
-
-}
-/////////////////////////////////////////////////////////////////////////////////
-
-void test_state_mach(void)
-{
- if(stepMode == QTR)
- {
- adv_state_mach_half();
- }
- else
- {
- adv_state_mach_full();
- }
-}
-///////////////////////////////////////////////////////////////////////////////////
-
-
-void set_speed(float speed)
-{
- if(motorState == RUN)
- {
- if(stepMode == QTR)
- {
- numRampSteps = speed;
- stepInterval_us = 1000000*(1.0/(speed * 8));
- advance_state.attach_us(&test_state_mach, stepInterval_us);
- }
- else
- {
- numRampSteps = speed;
- stepInterval_us = 1000000*(1.0/(speed * 4));
- advance_state.attach_us(&test_state_mach, stepInterval_us);
- }
- }
- else //not in run mode, save the change
- {
- if(stepMode == QTR)
- {
- numRampSteps = speed;
- stepInterval_us = 1000000*(1.0/(speed * 8));
- }
- else
- {
- stepInterval_us = 1000000*(1.0/(speed * 4));
- numRampSteps = speed;
- }
- }
-}