forked from seishuku/TeensyCNC
-
Notifications
You must be signed in to change notification settings - Fork 1
/
motor.c
238 lines (209 loc) · 7.55 KB
/
motor.c
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
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
// TeensyCNC
// Copyright 2016 Matt Williams
//
// Motor PID control and encoder handling
// Alun Jones - Interrupt disable/enable while enabling/disabling motor, was causing MCU crashing.
#include "MK20D10.h"
#include <stdlib.h>
#include "pwm.h"
#ifndef clamp
#define clamp(a, min, max) (((a)<(min))?(min):(((a)>(max))?(max):(a)))
#endif
const int8_t Quad_Table[4][4][4] =
{
{
{0, 1, -1, 3},
{-1, 0, 2, 1},
{1, -2, 0, -1},
{3, -1, 1, 0}
},
{
{0, 1, -1, -2},
{-1, 0, 3, 1},
{1, 3, 0, -1},
{2, -1, 1, 0}
},
{
{0, 1, -1, 2},
{-1, 0, 3, 1},
{1, 3, 0, -1},
{-2, -1, 1, 0}
},
{
{0, 1, -1, 3},
{-1, 0, -2, 1},
{1, 2, 0, -1},
{3, -1, 1, 0}
}
};
/*
0x0040 = 0000 0000 0100 0000 = PORT C6 - Encoder XA
0x0080 = 0000 0000 1000 0000 = PORT C7 - Encoder XB
0x0001 = 0000 0000 0000 0001 = PORT B0 - Encoder YB
0x0002 = 0000 0000 0000 0010 = PORT B1 - Encoder YA
*/
// Slots encoder pin status into a bit field, as a look up into Quad_Table for quadrature directional information
// Returns 0, 1, 2, or 3, depending on which opto sensor is blocked and when.
#define XENCODER_GET_PINS() ((GPIOC->PDIR & 0x00C0U) >> 6U) // Encoder XB/XA
#define YENCODER_GET_PINS() ((GPIOB->PDIR & 0x0003U) >> 0U) // Encoder YA/YB
// Current encoder quadratic value
uint8_t encoderQuadX;
uint8_t encoderQuadY;
// Previous encoder quadratic value
uint8_t encoderPrevQuadX;
uint8_t encoderPrevQuadY;
// Encoder coords to target (these do the moving)
volatile int32_t targetX = 0;
volatile int32_t targetY = 0;
volatile int32_t encoderPosX;
volatile int32_t encoderPosY;
// Set X axis motor PWM, neg values run opposite direction
void MotorCtrlX (int32_t PWM) {
if (PWM > 0) {
PWM_SetRatio(0x05, clamp((uint16_t) 65535 - abs(PWM), 0, 65535));
PWM_SetRatio(0x06, 65535);
} else {
PWM_SetRatio(0x05, 65535);
PWM_SetRatio(0x06, clamp((uint16_t) 65535 - abs(PWM), 0, 65535));
}
}
// Same, but Y axis
void MotorCtrlY (int32_t PWM) {
if (PWM > 0) {
PWM_SetRatio(0x00, clamp((uint16_t) 65535 - abs(PWM), 0, 65535));
PWM_SetRatio(0x01, 65535);
} else {
PWM_SetRatio(0x00, 65535);
PWM_SetRatio(0x01, clamp((uint16_t) 65535 - abs(PWM), 0, 65535));
}
}
// X encoder interrupt
void __attribute__ ((interrupt)) Cpu_ivINT_PORTC (void) {
// Check for interrupt flag for either input
if ((PORTC->PCR[6] & PORT_PCR_ISF_MASK) || (PORTC->PCR[7] & PORT_PCR_ISF_MASK)) {
// Clear the flag(s)
PORTC->PCR[6] |= PORT_PCR_ISF_MASK;
PORTC->PCR[7] |= PORT_PCR_ISF_MASK;
// Get the encoder status
uint8_t c12 = XENCODER_GET_PINS();
// Retreive directional data from quadrature lookup table
int8_t new_step = Quad_Table[encoderPrevQuadX][encoderQuadX][c12];
// Store the previous, last value
encoderPrevQuadX = encoderQuadX;
// Store the current, last value
encoderQuadX = c12;
if (new_step == 3) { // 3 is an error
// Ignore error
} else if (new_step != 0) { // It's good?
encoderPosX += new_step; // Count it in whatever direction it's going
}
}
}
// Y encoder interrupt, exactly as X axis
void __attribute__ ((interrupt)) Cpu_ivINT_PORTB (void) {
if ((PORTB->PCR[0] & PORT_PCR_ISF_MASK) || (PORTB->PCR[1] & PORT_PCR_ISF_MASK)) {
PORTB->PCR[0] |= PORT_PCR_ISF_MASK;
PORTB->PCR[1] |= PORT_PCR_ISF_MASK;
uint8_t c12 = YENCODER_GET_PINS();
int8_t new_step = Quad_Table[encoderPrevQuadY][encoderQuadY][c12];
encoderPrevQuadY = encoderQuadY;
encoderQuadY = c12;
if (new_step == 3) {
// Ignore error
} else if (new_step != 0) {
encoderPosY += new_step;
}
}
}
// PID stuff
// Position multiplier
#define KP 5000.0f
// Derivative multiplier
#define KD 24000.0f
// Previous derivative error
int32_t lastErrorX = 0;
int32_t lastErrorY = 0;
void __attribute__ ((interrupt)) Cpu_ivINT_FTM1 (void) {
// Is the overflow interrupt flag pending? (measured at approx 3500 Hz)
if (FTM1->SC & FTM_SC_TOF_MASK) {
// Clear flag
FTM1->SC &= ~FTM_SC_TOF_MASK;
// Run proportional control
// find the error term of current position - target
int32_t errorX = targetX - encoderPosX;
int32_t errorY = targetY - encoderPosY;
//generalized PID formula
//correction = Kp * error + Kd * (error - prevError)
MotorCtrlX(KP * errorX + KD * (float) (errorX - lastErrorX));
MotorCtrlY(KP * errorY + KD * (float) (errorY - lastErrorY));
// Store previous error
lastErrorX = errorX;
lastErrorY = errorY;
}
}
// Sets PID interrupt to system clock, enabling it.
void MotorEnable (void) {
lastErrorX = 0;
lastErrorY = 0;
__disable_irq();
FTM1->SC = (FTM1->SC & (~(FTM_SC_CLKS_MASK & FTM_SC_TOF_MASK))) | (0x08U);
FTM1->SC = FTM_SC_TOIE_MASK | FTM_SC_CLKS(0x02) | FTM_SC_PS(0x00);
__enable_irq();
}
// Removes clock source from PID interrupt timer, disabling it.
// Also sets axis motors to 0 PWM.
void MotorDisable (void) {
__disable_irq();
FTM1->SC = (FTM1->SC & (~(FTM_SC_CLKS_MASK & FTM_SC_TOF_MASK))) | (0x00U);
FTM1->SC = FTM_SC_TOIE_MASK | FTM_SC_CLKS(0x00) | FTM_SC_PS(0x00);
__enable_irq();
MotorCtrlX(0);
MotorCtrlY(0);
}
void Motor_Init (void) {
// Initialize enocder inputs with interrupts on both edges
// PB0/PB1 = Y A/B encoder input
PORTB->PCR[0] = (PORTB->PCR[0] & ~(PORT_PCR_ISF_MASK | PORT_PCR_MUX(0x06))) | PORT_PCR_MUX(0x01);
PORTB->PCR[0] = (PORTB->PCR[0] & ~(PORT_PCR_IRQC(0x04))) | (PORT_PCR_ISF_MASK | PORT_PCR_IRQC(0x0B));
PORTB->PCR[1] = (PORTB->PCR[1] & ~(PORT_PCR_ISF_MASK | PORT_PCR_MUX(0x06))) | PORT_PCR_MUX(0x01);
PORTB->PCR[1] = (PORTB->PCR[1] & ~(PORT_PCR_IRQC(0x04))) | (PORT_PCR_ISF_MASK | PORT_PCR_IRQC(0x0B));
NVIC_SetPriority(PORTB_IRQn, 0x50);
NVIC_EnableIRQ(PORTB_IRQn);
// PC6/PC7 = X A/B encoder input
PORTC->PCR[6] = (PORTC->PCR[6] & ~(PORT_PCR_ISF_MASK | PORT_PCR_MUX(0x06))) | PORT_PCR_MUX(0x01);
PORTC->PCR[6] = (PORTC->PCR[6] & ~(PORT_PCR_IRQC(0x04))) | (PORT_PCR_ISF_MASK | PORT_PCR_IRQC(0x0B));
PORTC->PCR[7] = (PORTC->PCR[7] & ~(PORT_PCR_ISF_MASK | PORT_PCR_MUX(0x06))) | PORT_PCR_MUX(0x01);
PORTC->PCR[7] = (PORTC->PCR[7] & ~(PORT_PCR_IRQC(0x04))) | (PORT_PCR_ISF_MASK | PORT_PCR_IRQC(0x0B));
NVIC_SetPriority(PORTC_IRQn, 0x50);
NVIC_EnableIRQ(PORTC_IRQn);
// Initialize interrupt timer for PID control
SIM->SCGC6 |= SIM_SCGC6_FTM1_MASK;
// Set up mode register
FTM1->MODE = FTM_MODE_FAULTM(0x00) | FTM_MODE_WPDIS_MASK;
// Clear status and control register
FTM1->SC = FTM_SC_CLKS(0x00) | FTM_SC_PS(0x00);
// Clear counter initial register
FTM1->CNTIN = FTM_CNTIN_INIT(0x00);
// Reset counter register
FTM1->CNT = FTM_CNT_COUNT(0x00);
// Clear channel status and control register
FTM1->CONTROLS[0].CnSC = 0x00;
// Clear channel status and control register
FTM1->CONTROLS[1].CnSC = 0x00;
// Set up modulo register
// Bus clock / Freq = FTM1_MOD
// 36MHz / Freq = FTM1_MOD
// MOD = 9 = 4000000Hz (4Mhz)
// WRH: I think tha actual input to FTM1 32,000 Hz.
// - Reference sections 3.8.2.3 and 5.3 of the "K20 Sub-Family Reference Manual"
FTM1->MOD = FTM_MOD_MOD(9 - 1);
NVIC_SetPriority(FTM1_IRQn, 0x10);
NVIC_EnableIRQ(FTM1_IRQn);
// Set up status and control register
FTM1->SC = FTM_SC_TOIE_MASK | FTM_SC_CLKS(0x02) | FTM_SC_PS(0x00);
// Initialize encoder variables
encoderQuadX = XENCODER_GET_PINS();
encoderPrevQuadX = encoderQuadX;
encoderQuadY = YENCODER_GET_PINS();
encoderPrevQuadY = encoderQuadY;
}