Self_Driving_Rover/Wheels_Controller.cpp at master · vrrs/Self_Driving_Rover · GitHub

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
#include "Arduino.h"
#include <Wheels_Controller.h>

struct pt                  Wheels_Controller::ptM1;
struct pt                  Wheels_Controller::ptM2;
volatile unsigned long	   Wheels_Controller::FREQ1[2];
volatile int               Wheels_Controller::PIN[2];
volatile bool              Wheels_Controller::flag[2];
volatile unsigned long     Wheels_Controller::startime[2];
volatile unsigned long     Wheels_Controller::PERIOD;
int                        Wheels_Controller::DIRECTION;
volatile bool              Wheels_Controller::MOVING;

Wheels_Controller::Wheels_Controller(){
	PT_INIT(&ptM1);
	PT_INIT(&ptM2);

	reset();
}

void Wheels_Controller::set_direction(int direction){
	Wheels_Controller::DIRECTION=direction;
	int pin1=0;
	int pin2=0;
	if(direction==consts.FORWARD){
			pin1=consts.MOTOR1_CLOCKWISE;
			pin2=consts.MOTOR2_COUNTERCLOCKWISE;
	}
	else{
		if(direction==consts.BACKWARD){
			pin2=Wheels_Controller::consts.MOTOR2_CLOCKWISE;
			pin1=Wheels_Controller::consts.MOTOR1_COUNTERCLOCKWISE;
		}
	}
	Wheels_Controller::PIN[0]=pin1;
	Wheels_Controller::PIN[1]=pin2;
}

void Wheels_Controller::start_moving(int direction,unsigned long m1u1,unsigned long m2u1){
	if(!MOVING){
		FREQ1[0]=m1u1;
		FREQ1[1]=m2u1;
		set_direction(direction);
		MOVING=true;
	}
}
unsigned long Wheels_Controller::enforce_max_speed(unsigned long v){
	if(v>consts.PERIOD_CYCLE){
		return consts.PERIOD_CYCLE;
	}
	if(v<0L){
		return 0L;
	}
	return v;
}

void Wheels_Controller::set_freqs(unsigned long m1v,unsigned long m2v){
	FREQ1[0]=enforce_max_speed(m1v);
	FREQ1[1]=enforce_max_speed(m2v);
}

void Wheels_Controller::stop_moving(){
	MOVING=false;
}

void Wheels_Controller::reset(){
	PERIOD=consts.PERIOD_CYCLE;
	for(int i=0;i<2;i++){
		FREQ1[i]=100L;
		flag[i]=true;
		startime[i]=0L;
	}
	MOVING=false;
	DIRECTION=consts.FORWARD;

}

void Wheels_Controller::schedule_wheel_motion(){
	if(MOVING){
		moveWheel0(&ptM1);
		moveWheel1(&ptM2);
	}
	else{
		digitalWrite(PIN[0],LOW);
		digitalWrite(PIN[1],LOW);
		reset();
	}
}

int Wheels_Controller::moveWheel0(struct pt *ptt){
	PT_BEGIN(ptt);
	while(1){
		PT_WAIT_UNTIL(ptt,flag[0] || (( micros() - startime[0]) > (PERIOD - FREQ1[0] )));
		if (!flag[0])	{
			startime[0]=micros();
			flag[0]=true;
		}
		digitalWrite(PIN[0],HIGH);
		PT_WAIT_UNTIL(ptt, (micros() - startime[0]) > FREQ1[0]);
		digitalWrite(PIN[0],LOW);
		startime[0]=micros();
		flag[0]=false;
	}
	PT_END(ptt);
}

int Wheels_Controller::moveWheel1(struct pt *ptt){
	PT_BEGIN(ptt);
	while(1){
		PT_WAIT_UNTIL(ptt,flag[1] || ( (micros() - startime[1]) > (PERIOD - FREQ1[1]) ));
		if (!flag[1])	{
			startime[1]=micros();
			flag[1]=true;
		}
		digitalWrite(PIN[1],HIGH);
		PT_WAIT_UNTIL(ptt, (micros() - startime[1]) > FREQ1[1]);
		digitalWrite(PIN[1],LOW);
		startime[1]=micros();
		flag[1]=false;
	}
	PT_END(ptt);
}

unsigned long Wheels_Controller::get_motor1_freq(){
	return FREQ1[0];
}

unsigned long Wheels_Controller::get_motor2_freq(){
	return FREQ1[1];
}