forked from abunai59/umouse2013
-
Notifications
You must be signed in to change notification settings - Fork 0
/
Motors.c
124 lines (115 loc) · 2.87 KB
/
Motors.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
// contains functions for configuration
// and initialization
#include <p33FJ128MC802.h>
#include "main.h"
#include "motors.h"
#include "config.h"
// Use timer 1 for right motor
void __attribute__((interrupt, no_auto_psv)) _T1Interrupt(void)
{
// T1CONbits.TON = 0; // turn off TMR1
if(DIR == FORWARD || DIR == LEFT)
{
// pulse right motor forward
R_IND += 1;
R_IND %= 4;
PORTB = (PORTB & RS_AND) + R_SEQ[R_IND];
ST_COUNT ++; // increment turn steps
if(DIR == LEFT && ST_COUNT == L_STEP) // when enough steps are taken
{
DIR = STOP; // stop turning
ST_COUNT=0; // reset turn count
}
if(DIR == FORWARD && ST_COUNT == F_STEP) // when enough steps are taken
{
DIR = STOP; // stop turning
ST_COUNT=0; // reset turn count
PR1 = PR1_MAP;
PR2 = PR1_MAP;
}
}
else if(DIR == BACKWARD || DIR == RIGHT || DIR == TURN)
{
// pulse right motor reverse direction
R_IND += 3;
R_IND %= 4;
PORTB = (PORTB & RS_AND) + R_SEQ[R_IND];
// if(DIR == RIGHT || DIR == TURN)
// {
ST_COUNT ++; // increment turn steps
// when enough steps are taken
if(DIR == RIGHT && ST_COUNT == R_STEP)
{
DIR = STOP; // stop turning
ST_COUNT=0; // reset turn count
}
if(DIR == TURN && ST_COUNT == T_STEP)
{
DIR = STOP; // stop turning
ST_COUNT=0; // reset turn count
}
// if(DIR == BACKWARD && ST_COUNT == B_STEP)
// {
// DIR = STOP; // stop turning
// ST_COUNT=0; // reset turn count
// }
// }
}
else if( DIR == R_FORE) {
// pulse right motor forward
R_IND += 1;
R_IND %= 4;
PORTB = (PORTB & RS_AND) + R_SEQ[R_IND];
DIR = STOP;
}
else if( DIR == R_BACK) {
// pulse right motor backward
R_IND += 3;
R_IND %= 4;
PORTB = (PORTB & RS_AND) + R_SEQ[R_IND];
DIR = STOP;
}
// stop
else {
}
IFS0bits.T1IF = 0; // turn off TMR1 flag
// T1CONbits.TON = 1;
}
// Use timer 2 for left motor
void __attribute__((interrupt, no_auto_psv)) _T2Interrupt(void)
{
// T2CONbits.TON = 0; // turn off TMR1
if(DIR == FORWARD || DIR == RIGHT || DIR == TURN)
{
// pulse left motor forward
L_IND += 1;
L_IND %= 4;
PORTB = (PORTB & LS_AND) + L_SEQ[L_IND];
}
else if(DIR == BACKWARD || DIR == LEFT)
{
// pulse left motor reverse direction
L_IND += 3;
L_IND %= 4;
PORTB = (PORTB & LS_AND) + L_SEQ[L_IND];
}
else if(DIR == L_FORE) {
// pulse left motor forward
L_IND += 1;
L_IND %= 4;
PORTB = (PORTB & LS_AND) + L_SEQ[L_IND];
DIR = STOP;
}
else if(DIR == L_BACK) {
// pulse left motor backward
L_IND += 3;
L_IND %= 4;
PORTB = (PORTB & LS_AND) + L_SEQ[L_IND];
DIR = STOP;
}
// stop
else {
}
IFS0bits.T2IF = 0; // turn off TMR2 flag
// T2CONbits.TON = 1;
}