1+ #include < MODSPI.h>
2+
3+ // MODSPI - Homing Routine Example
4+ // This is an example of how todo advanced homing with the MODSPI system
5+ // The routine looks for a rising edge of a homing switch, moving the motor towards it
6+ // Once a rising edge is found, it backs away at a slower speed to find the falling edge
7+ // The axis then moves towards a park position. Once parked it moves to a far away position and then
8+ // back towards the homing switch where it repeats the loop.
9+ //
10+ // Note: this code is also in MM and converts to Steps.
11+ // This routine requires a stepper motor module & a digital inputs module
12+ // It is only intended as a guide to help you test and structure your code.
13+ // ---------------------------------------------------------------------------------------------
14+
15+ // Create an instance of the MODSPI_StepperController class named "motor1".
16+ MODSPI_StepperController motor1;
17+ // Create an instance of the MODSPI_DigitalInputs class named "inputs1"
18+ MODSPI_DigitalInputs inputs1;
19+
20+ const float motorRatedCurrent = 1.0 ; // The maximum current of the motor specified.
21+ const float runCurrent = 0.8 ; // The current used when the motor is moving
22+ const float idleCurrent = 0.2 ; // The current used when the motor is idle.
23+
24+ float stepsPerRotation = 200 ; // Most motors have 200 steps per revolution
25+ float mmPerRotation = 40 ; // 20tooth x 2mm pitch
26+ float stepsPerMM = stepsPerRotation / mmPerRotation;
27+
28+ float speedMM = 200 ; // Move at 200mm per second
29+ float accelMM = 200 ; // Accelerate at 200mm per second
30+ float homeSpeedMM = 40 ; // Home at 80mm per second
31+ float homeAccelMM = 200 ; // Home accel at 200mm per second
32+
33+ float travelLimitMM = 300 ; // max travel of 300mm
34+ float parkPositionMM = 5 ; // parked poisition of 5mm
35+ float OutPositionMM = 250 ; // move motor to out position
36+ float MidPositionMM = 50 ; // Position near the homing
37+
38+ float speedSteps = (speedMM / mmPerRotation) * stepsPerRotation;
39+ float accelSteps = (accelMM / mmPerRotation) * stepsPerRotation;
40+ float homeSpeedSteps = (homeSpeedMM / mmPerRotation) * stepsPerRotation;
41+ float homeAccelSteps = (accelMM / mmPerRotation) * stepsPerRotation;
42+ float travelLimitSteps = (travelLimitMM / mmPerRotation) * stepsPerRotation;
43+ float parkPositionSteps = (parkPositionMM / mmPerRotation) * stepsPerRotation;
44+ float OutPositionSteps = (OutPositionMM / mmPerRotation) * stepsPerRotation;
45+ float MidPositionSteps = (MidPositionMM / mmPerRotation) * stepsPerRotation;
46+
47+ enum HomingState {
48+ UNKNOWN,
49+ LOOKING_FOR_RISING_EDGE,
50+ LOOKING_FOR_FALLING_EDGE,
51+ PARKING,
52+ HOMED,
53+ FAILED
54+ };
55+ HomingState homingState1;
56+
57+
58+ enum MovingState {
59+ NONE,
60+ MovingOut,
61+ MovingIn
62+ };
63+ MovingState movingState1;
64+
65+
66+ void setup () {
67+ motor1.begin (bay1); // Stepper Motor module is inserted into bay1 (chip select D5)
68+ inputs1.begin (bay2); // Digital Input module is inserted into bay2 (chip select D6)
69+
70+ // Optional settings
71+ motor1.setMaxSpeed (speedSteps); // Set speed to 2 revolutions per second.
72+ motor1.setAcceleration (accelSteps); // set acceleration to 4 revolutions per second per second.
73+ motor1.setCurrent (motorRatedCurrent, runCurrent, idleCurrent); // Set the currents of the motor
74+ motor1.setCurrentPosition (0 ); // reset the current position to 0.
75+ }
76+
77+ void loop () {
78+ delay (1 ); // Wait 1ms
79+
80+ if (homingState1 != HOMED) {
81+ homeMotor1 (); // Home Motor routine
82+ } else {
83+ if (motor1.isTargetPositionReached ()) {
84+ homingState1 = UNKNOWN;
85+ }
86+ }
87+ }
88+
89+ void homeMotor1 () {
90+ bool motor1HomeSwitchState = debounceSwitch (7 );
91+
92+ switch (homingState1) {
93+ case UNKNOWN:
94+ motor1.fastStop (); // Quickly stops the motor
95+ motor1.setCurrentPosition (0 ); // reset the current position
96+ Serial.println (" Starting homing." );
97+ motor1.setMaxSpeed (homeSpeedSteps); // Set speed to homing speed
98+ motor1.setAcceleration (homeAccelSteps); // set acceleration to homing accleration
99+ motor1.setTargetPosition (-travelLimitSteps); // move backwards towards homing switch
100+ homingState1 = LOOKING_FOR_RISING_EDGE;
101+ break ;
102+
103+ case LOOKING_FOR_RISING_EDGE:
104+ if (motor1HomeSwitchState == HIGH) {
105+ Serial.println (" Rising Edge found! Now looking for falling edge." );
106+ motor1.fastStop (); // quickly stop the motor
107+ motor1.setCurrentPosition (0 ); // reset the current position
108+ motor1.setMaxSpeed (homeSpeedSteps / 5 ); // set speed to 1/5th speed of homing
109+ motor1.setAcceleration (homeAccelSteps / 5 ); // set acceleration to 1/5th the homing accel
110+ motor1.setTargetPosition (travelLimitSteps); // move forwards slowly away from homing switch
111+ homingState1 = LOOKING_FOR_FALLING_EDGE;
112+ } else if (motor1.isTargetPositionReached ()) {
113+ Serial.println (" Failed to find homing switch rising edge within travel limit" );
114+ homingState1 = FAILED;
115+ }
116+ break ;
117+
118+ case LOOKING_FOR_FALLING_EDGE:
119+ if (motor1HomeSwitchState == LOW) {
120+ Serial.println (" Falling Edge found! Now parking." );
121+ motor1.fastStop (); // Quickly stop the motor
122+ motor1.setCurrentPosition (0 ); // reset the position
123+ motor1.setMaxSpeed (speedSteps); // set speed to normal speed
124+ motor1.setAcceleration (accelSteps); // set acceleration to normal acceleration
125+ motor1.setTargetPosition (parkPositionSteps); // move forwards to park position away from homing switch
126+ homingState1 = PARKING;
127+ } else if (motor1.isTargetPositionReached ()) {
128+ Serial.println (" Failed to find homing switch falling edge within travel limit" );
129+ homingState1 = FAILED;
130+ }
131+ break ;
132+
133+ case PARKING:
134+ if (motor1.isTargetPositionReached ()) {
135+ Serial.println (" Homing Finished" );
136+ homingState1 = HOMED;
137+ }
138+ break ;
139+ }
140+ }
141+
142+ void moveMotor1 () {
143+ if (motor1.isTargetPositionReached ()) {
144+ switch (movingState1) {
145+ case NONE:
146+ motor1.setTargetPosition (OutPositionSteps); // move to out position
147+ movingState1 = MovingOut;
148+ break ;
149+ case MovingOut:
150+ motor1.setTargetPosition (MidPositionSteps); // move to mid position near homing
151+ movingState1 = MovingIn;
152+ break ;
153+ case MovingIn:
154+ movingState1 = NONE; // stop moving and start homing
155+ homingState1 = UNKNOWN; // reset routine to start homing again
156+ break ;
157+ }
158+ }
159+ }
160+
161+ bool debounceSwitch (int Din) {
162+ // Sometimes the digital input is a little noisy. This function just adds a delay before reading again.
163+ // Optional, but helps with reliability of homing.
164+ bool startState = inputs1.read (Din);
165+ delay (5 );
166+ return inputs1.read (Din);
167+ }
0 commit comments