Roboner/Drive.cpp at master · liadpaz/Roboner · 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
#include "Roboner.h"

void Roboner::forward(int gainL, int gainR) {
	analogWrite(MLB, 0);
	analogWrite(MRB, 0);
	analogWrite(MLF, map(gainL, 0, 100, 0, 255)); //255 max L ENGINE
	analogWrite(MRF, map(gainR, 0, 100, 0, 242)); //242 max R ENGINE
}

void Roboner::backward(int gainL, int gainR) {
	analogWrite(MLF, 0);
	analogWrite(MRF, 0);
	analogWrite(MLB, map(gainL, 0, 100, 0, 255)); //255 max L ENGINE
	analogWrite(MRB, map(gainR, 0, 100, 0, 242)); //242 max R ENGINE
}

void Roboner::stopFor(double sec) {
	analogWrite(MLF, 0);
	analogWrite(MRF, 0);
	analogWrite(MLB, 0);
	analogWrite(MRB, 0);
	delay(sec * 1000);
}

void Roboner::DriveR(int Speed) {
	double fix = USFR - USRR;	/*Difference between front and back.
	* Positive -> front is farther
	* Negative -> back is farther
	*/
	if (USFR > sideWall) {	//Robot is too far from wall
		if (fix > 0)
			forward(Speed + diff + fixWall, Speed - diff);
		else
			forward(Speed - diff + fixWall, Speed + diff);
	}
	else {	//Robot is too close to wall
		if (fix > 0)
			forward(Speed + diff, Speed - diff + fixWall);
		else
			forward(Speed - diff, Speed + diff + fixWall);
	}
}

void Roboner::DriveL(int Speed) {
	double fix = USFL - USRL;	/*Difference between front and back.
	* Positive -> front is farther
	* Negative -> back is farther
	*/
	if (USFL > sideWall) {	//Robot is too far from wall
		if (fix > 0)
			forward(Speed - diff, Speed + diff + fixWall);
		else
			forward(Speed + diff, Speed - diff + fixWall);
	}
	else {	//Robot is too close to wall
		if (fix > 0)
			forward(Speed - diff + fixWall, Speed + diff);
		else
			forward(Speed + diff + fixWall, Speed - diff);
	}
}

void Roboner::BDriveR(int Speed) {
	double fix = USRR - USFR;	/*Difference between front and back.
	* Positive -> back is farther
	* Negative -> front is farther
	*/
	if (USFR > sideWall) {	//Robot is too far from wall
		if (fix > 0)
			backward(Speed + diff + fixWall, Speed - diff);
		else
			backward(Speed - diff + fixWall, Speed + diff);
	}
	else {	//Robot is too close to wall
		if (fix > 0)
			backward(Speed + diff, Speed - diff + fixWall);
		else
			backward(Speed - diff, Speed + diff + fixWall);
	}
}