-
Notifications
You must be signed in to change notification settings - Fork 0
/
motor_control.c
152 lines (127 loc) · 2.69 KB
/
motor_control.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
/*
* motor_control.c
*
* Created on: 28/08/2015
* Author: Ryan Taylor
*/
#include "inc/hw_types.h"
#include <stdio.h>
#include <stdint.h>
#include <stdbool.h>
#include <math.h>
#include "inc/hw_gpio.h"
#include "inc/hw_memmap.h"
#include "inc/hw_ints.h"
#include "driverlib/gpio.h"
#include "driverlib/systick.h"
#include "driverlib/interrupt.h"
#define GPIOHigh(x) GPIOPinWrite(GPIO_PORTF_BASE, x, x)//GPIO_PIN_1
#define GPIOLow(x) GPIOPinWrite(GPIO_PORTF_BASE, x, 0)
#define MAX_24BIT_VAL 0X0FFFFFF
#define FACTOR 13000000
int time_last = 0;
int thisStep = 0;
void stepper_motor(int direction){
switch (thisStep) {
case 0: // 1010
GPIOHigh(GPIO_PIN_1);
GPIOLow(GPIO_PIN_2);
GPIOHigh(GPIO_PIN_3);
GPIOLow(GPIO_PIN_4);
if (direction == 0){
thisStep = 1;
}
else {
thisStep = 3;
}
break;
case 1: // 0110
GPIOLow(GPIO_PIN_1);
GPIOHigh(GPIO_PIN_2);
GPIOHigh(GPIO_PIN_3);
GPIOLow(GPIO_PIN_4);
if (direction == 0){
thisStep = 2;
}
else {
thisStep = 0;
}
break;
case 2: //0101
GPIOLow(GPIO_PIN_1);
GPIOHigh(GPIO_PIN_2);
GPIOLow(GPIO_PIN_3);
GPIOHigh(GPIO_PIN_4);
if (direction == 0){
thisStep = 3;
}
else {
thisStep = 1;
}
break;
case 3: //1001
GPIOHigh(GPIO_PIN_1);
GPIOLow(GPIO_PIN_2);
GPIOLow(GPIO_PIN_3);
GPIOHigh(GPIO_PIN_4);
if (direction == 0){
thisStep = 0;
}
else {
thisStep = 2;
}
break;
}
}
int step_mode_return(void){
return thisStep;
}
void stepper_motor_off(void){
GPIOHigh(GPIO_PIN_1);
GPIOHigh(GPIO_PIN_2);
GPIOHigh(GPIO_PIN_3);
GPIOHigh(GPIO_PIN_4);
}
float stepper_system(float time_step){
float time = 0;
if (time_step > 300){
time_step = 300;
}
if (time_step != 0){
time = 1/time_step;
}
if ( time_step == 0){
time = 0;
}
return time;
}
int step(float time_step){
int current_time = SysTickValueGet();
int diff = 0;
int direction = 0; //0 = clockwise, 1 = anti clockwise
if (time_step < 0){
direction = 1;
time_step = -1*time_step;
}
//if time less then 0, change direction
if (current_time <= time_last){
diff = time_last - current_time;
}
else {
diff = (time_last + MAX_24BIT_VAL) - current_time;
}
if (diff > (time_step*FACTOR) && time_step != 0) {
stepper_motor(direction);
time_last = current_time;
}
return current_time;
}
float step_motor_control(int encoder, int aim_pos){
int error = 0;
float time_step = 0;
error = aim_pos - encoder;
time_step = 10*error; //gain
time_step = stepper_system(time_step);
step(time_step);
return error;
}