-
Notifications
You must be signed in to change notification settings - Fork 18
/
configuration.ino
122 lines (95 loc) · 3.11 KB
/
configuration.ino
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
/**
* Polargraph Server. - CORE
* Written by Sandy Noble
* Released under GNU License version 3.
* http://www.polargraph.co.uk
* https://github.com/euphy/polargraph_server_polarshield
Configuration.
This is one of the core files for the polargraph server program.
It sets up the motor objects (AccelSteppers), and has default
values for the motor, sprocket and microstepping combinations used
by polargraphs so far.
*/
// =================================================================
// Polarshield motor driver board
// This uses stepstick-format stepper drivers on arduino pins 3 to 8.
// Motor C has never been used!
#if MOTHERBOARD == POLARSHIELD
#define MOTOR_A_ENABLE_PIN 3
#define MOTOR_A_STEP_PIN 4
#define MOTOR_A_DIR_PIN 5
#define MOTOR_B_ENABLE_PIN 6
#define MOTOR_B_STEP_PIN 7
#define MOTOR_B_DIR_PIN 8
#define MOTOR_C_ENABLE_PIN 30
#define MOTOR_C_STEP_PIN 31
#define MOTOR_C_DIR_PIN 32
#elif MOTHERBOARD == RAMPS14
// Uses E1 driver on RAMPS
#define MOTOR_A_ENABLE_PIN 30
#define MOTOR_A_STEP_PIN 36
#define MOTOR_A_DIR_PIN 34
// Uses Y motor driver on RAMPS
#define MOTOR_B_ENABLE_PIN 56
#define MOTOR_B_STEP_PIN 60
#define MOTOR_B_DIR_PIN 61
#elif MOTHERBOARD == TFTSHIELD
// Uses E1 driver on RAMPS
#define MOTOR_A_ENABLE_PIN 21
#define MOTOR_A_STEP_PIN 20
#define MOTOR_A_DIR_PIN 19
// Uses Y motor driver on RAMPS
#define MOTOR_B_ENABLE_PIN 18
#define MOTOR_B_STEP_PIN 17
#define MOTOR_B_DIR_PIN 16
#endif
AccelStepper motorA(1,MOTOR_A_STEP_PIN, MOTOR_A_DIR_PIN);
AccelStepper motorB(1,MOTOR_B_STEP_PIN, MOTOR_B_DIR_PIN);
//AccelStepper motorC(1,MOTOR_C_STEP_PIN, MOTOR_C_DIR_PIN);
void configuration_motorSetup()
{
#ifdef DEBUG
Serial.print(F("A: En:"));
Serial.print(MOTOR_A_ENABLE_PIN);
Serial.print(F(", St:"));
Serial.print(MOTOR_A_STEP_PIN);
Serial.print(F(", Di:"));
Serial.println(MOTOR_A_DIR_PIN);
Serial.print(F("B: En:"));
Serial.print(MOTOR_B_ENABLE_PIN);
Serial.print(F(", St:"));
Serial.print(MOTOR_B_STEP_PIN);
Serial.print(F(", Di:"));
Serial.println(MOTOR_B_DIR_PIN);
#endif
pinMode(MOTOR_A_ENABLE_PIN, OUTPUT);
digitalWrite(MOTOR_A_ENABLE_PIN, HIGH);
pinMode(MOTOR_B_ENABLE_PIN, OUTPUT);
digitalWrite(MOTOR_B_ENABLE_PIN, HIGH);
motorA.setEnablePin(MOTOR_A_ENABLE_PIN);
motorA.setPinsInverted(false, false, true);
motorB.setEnablePin(MOTOR_B_ENABLE_PIN);
motorB.setPinsInverted(true, false, true); // this one turns the opposite direction to A, hence inverted.
}
void configuration_setup()
{
mmPerStep = mmPerRev / multiplier(motorStepsPerRev);
stepsPerMM = multiplier(motorStepsPerRev) / mmPerRev;
// init SD card
sd_initSD();
lcd_initLCD();
lcd_showSummary();
delay(1000);
pinMode(2, INPUT);
touch.InitTouch();
touch.setPrecision(PREC_MEDIUM);
// calibration pins
pinMode(ENDSTOP_X_MIN, INPUT_PULLUP);
pinMode(ENDSTOP_Y_MIN, INPUT_PULLUP);
pinMode(ENDSTOP_X_MAX, INPUT_PULLUP);
pinMode(ENDSTOP_Y_MAX, INPUT_PULLUP);
lcd_displayFirstMenu();
releaseMotors();
}
// end of Polarshield definition
// =================================================================