forked from TZlindra/ELEC291Project2
-
Notifications
You must be signed in to change notification settings - Fork 0
/
movement.c
158 lines (136 loc) · 3.72 KB
/
movement.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
#include "movement.h"
#define LEFT_MOTOR_LHS P2_4 //brown
#define LEFT_MOTOR_RHS P2_3 //black
#define RIGHT_MOTOR_LHS P2_2 //grey/brown
#define RIGHT_MOTOR_RHS P2_1 //red
volatile int count = 0;
volatile long int timer = 0;
int PWM_percent_y = 0;
int PWM_percent_x = 0;
float left_wheel = 0;
float right_wheel = 0;
float new_right_wheel;
int prev_PWM_percent_x = 0;
int prev_PWM_percent_y = 0;
int state = 0;
void TIMER5Init(void) {
// Initialize timer 5 for periodic interrupts
SFRPAGE=0x10;
TMR5CN0=0x00; // Stop Timer5; Clear TF5; WARNING: lives in SFR page 0x10
CKCON1|=0b_0000_0100; // Timer 5 uses the system clock
TMR5RL=(0x10000L-(SYSCLK/(2*TIMER_5_FREQ))); // Initialize reload value
TMR5=0xffff; // Set to reload immediately
EIE2|=0b_0000_1000; // Enable Timer5 interrupts
TR5=1; // Start Timer5 (TMR5CN0 is bit addressable)
}
void Timer5_ISR (void) interrupt INTERRUPT_TIMER5 {
int current_TR0 = TR0, current_TR5 = TR5;
TR0 = 0, TR5 = 0;
SFRPAGE=0x10;
TF5H = 0; // Clear Timer5 interrupt flag
// P1_2 = !P1_2;
//P1_3 = !P1_3;
//P2_1 = !P2_1;
if (state)
{
if (timer > 50000)
{
timer = 0;
state = 0;
PWM_percent_y = 0;
left_wheel = 0;
new_right_wheel = 0;
}
else if (timer > 20000)
{
PWM_percent_y = 100;
left_wheel = 75;
new_right_wheel = 75;
}
else
{
PWM_percent_y = 100;
left_wheel = 75;
new_right_wheel = 0;
}
timer++;
}
if (count > 100)
{
count = 0;
}
if (PWM_percent_y >= 0)
{
LEFT_MOTOR_LHS = (count > left_wheel ) ? 0:1;
RIGHT_MOTOR_LHS = (count > new_right_wheel) ? 0:1;
}
else
{
LEFT_MOTOR_LHS = (count > left_wheel) ? 1:0;
RIGHT_MOTOR_LHS = (count > new_right_wheel) ? 1:0;
}
count++;
if (current_TR0 == 1) TR0 = 1;
if (current_TR5 == 1) TR5 = 1;
}
void idle(void) {
LEFT_MOTOR_LHS = 0;
RIGHT_MOTOR_LHS = 0;
LEFT_MOTOR_RHS = 0;
RIGHT_MOTOR_RHS = 0;
}
void straight(void) {
LEFT_MOTOR_RHS = 0;
RIGHT_MOTOR_RHS = 0;
}
void backward(void) {
LEFT_MOTOR_RHS = 1;
RIGHT_MOTOR_RHS = 1;
}
void PWM_manager(float x_value, float y_value) {
if (x_value >= 0) // RIGHT TURN
{
left_wheel = abs(y_value);
right_wheel = (100 - abs(x_value)) * abs(y_value) / 100;
}
else if (x_value < 0) // LEFT TURN
{
left_wheel = (100 - abs(x_value)) * abs(y_value) / 100;
right_wheel = abs(y_value);
}
// to account for the RIGHT wheel being stronger than the LEFT wheel
if (abs(y_value) <= 25) new_right_wheel = 0.9*right_wheel;
else if (abs(y_value) <= 50) new_right_wheel = 0.95*right_wheel;
else if (abs(y_value) <= 75) new_right_wheel = 0.95*right_wheel;
else if (abs(y_value) <= 100) new_right_wheel = 0.95*right_wheel;
else new_right_wheel = 0.95*right_wheel;
}
void movement_manager(float PWM_percent_y, float prev_PWM_percent_y) {
if (prev_PWM_percent_y != PWM_percent_y)
{
if (PWM_percent_y >= 0)
{
straight();
}
else
{
backward();
}
}
}
void movement_init(void) {
idle();
}
void movement_loop(float x, float y) {
// printf("JDY x: %f, JDY y: %f\r\n", x, y);
PWM_percent_x = x;
PWM_percent_y = y;
// printf("PWM_percent x: %f, PWM_percent y: %f\r\n", x, y);
movement_manager(PWM_percent_y, prev_PWM_percent_y);
PWM_manager(PWM_percent_x, PWM_percent_y);
prev_PWM_percent_x = PWM_percent_x;
prev_PWM_percent_y = PWM_percent_y;
}
void parking(void) {
state = 1;
}