-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathDrive.ino
127 lines (101 loc) · 2.48 KB
/
Drive.ino
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
/*
Drive.ino File
For control the redirection
use different methods for your robot control programs
@Author Fabrice
*/
const int i_correction = 0;
//todo try out robot and "try and error" correction value
/**
* simple controlling the drive
* first looking area then control the drive
*/
void simpleDriveMode()
{
driveForward(100);
bool b_objectInFront = false;
bool b_objectLeft = false;
bool b_objectRight = false;
//looking area
b_objectLeft = searchForObjects(SERVO_LEFT);
delay(100);
b_objectInFront = searchForObjects(SERVO_MIDDLE);
delay(100);
b_objectRight = searchForObjects(SERVO_RIGHT);
delay(100);
//drive
if(b_objectInFront == false)
{
driveForward(100);
}
else
{
turnRight();
}
if(b_objectLeft == true)
{
driveRight(50,20);
}
if(b_objectRight == true)
{
driveLeft(50,20);
}
}
/*
i_speed is for controling speed in percentage (0-100)
i_turn is for controlling turning in percentage 0-100
*/
void driveLeft(int i_speed, int i_turn)
{
//Motor A forward @ calulated speed
digitalWrite(DIR_A, HIGH);
digitalWrite(BRAKE_A, LOW);
analogWrite(PWM_A, i_speed ); //Spins the motor on Channel A at calulated speed
//Motor B forward @ calulated speed
digitalWrite(DIR_B, HIGH);
digitalWrite(BRAKE_B, LOW);
analogWrite(PWM_B, i_speed + i_turn);
}
/*
i_speed is for controling speed in percentage (0-100)
i_turn is for controling turningn in percanteage 0-100
*/
void driveRight(int i_speed, int i_turn)
{
//Motor A forward @ calulated speed
digitalWrite(DIR_A, HIGH);
digitalWrite(BRAKE_A, LOW);
analogWrite(PWM_A, i_speed + i_turn);
//Motor B forward @ calulated speed
digitalWrite(DIR_B, HIGH);
digitalWrite(BRAKE_B, LOW);
analogWrite(PWM_B, i_speed );
}
/*
i_speed is for controling speed in percentage (0-100)
*/
void driveForward(int i_speed)
{
//Motor A forward @ calulated speed
digitalWrite(DIR_A, HIGH);
digitalWrite(BRAKE_A, LOW);
analogWrite(PWM_A, i_speed + i_correction);
//Motor B forward @ calulated speed
digitalWrite(DIR_B, HIGH);
digitalWrite(BRAKE_B, LOW);
analogWrite(PWM_B, i_speed);
}
/*
i_speed is for controling speed in percentage (0-100)
*/
void driveBackward(int i_speed)
{
//Motor A backward @ calulated speed
digitalWrite(DIR_A, LOW);
digitalWrite(BRAKE_A, LOW);
analogWrite(PWM_A, i_speed);
//Motor B backward @ calulated speed
digitalWrite(DIR_B, LOW);
digitalWrite(BRAKE_B, LOW);
analogWrite(PWM_B, i_speed);
}