-
Notifications
You must be signed in to change notification settings - Fork 2
/
Sine Wave
202 lines (156 loc) · 3.3 KB
/
Sine Wave
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
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
/*
SINE WAVE
*/
#define F_CPU 1000000UL //Buffer board's crystal is not working
#define trig 22
#define echo 3
#include <m2560/io.h>
#include <m2560/Servo.h>
#include <ModularLibrary/Cytron.h>
#define pwm1 4
#define pwm2 13
#define pwm3 10
#define pwm4 9
#define dir1 30
#define dir2 31
#define dir3 32
#define dir4 33
float k=0.12; // Constant for getting the sine wave
int ang1=120,ang2=120,ang3=120,ang4=120; //Initial angles of servo motors for all wheels to be parallel
int v,y,z,w; //Angles of servo
int dist; //Distance in mm from wall
/********************Objects***********************/
servo servo1; // 4 Objects created for 4 servos
servo servo2;
servo servo3;
servo servo4;
Cytron motors(dir1,pwm1,dir2,pwm2,dir3,pwm3,dir4,pwm4);
void angle1(int dist)
{
v=(ang1-90) +(dist-1250)*k;
y=(ang2-90)+ (dist-1250)*k;
z=(ang3-90)+ (dist-1250)*k;
w=(ang4-90)+ (dist-1250)*k;
servo1.write(v);
servo2.write(y);
servo3.write(z);
servo4.write(w);
}
void angle2(int dist)
{
v=ang1 + (dist-2000)*k;
y=ang2 + (dist-2000)*k;
z=ang3 + (dist-2000)*k;
w=ang4 + (dist-2000)*k;
servo1.write(v);
servo2.write(y);
servo3.write(z);
servo4.write(w);
}
void angle3(int dist)
{
v=(ang1+90)-(dist-2750)*k;
y=(ang2+90)-(dist-2750)*k;
z=(ang3+90)-(dist-2750)*k;
w=(ang4+90)-(dist-2750)*k;
servo1.write(v);
servo2.write(y);
servo3.write(z);
servo4.write(w);
}
void angle4(int dist)
{
v=ang1-(dist-500)*k;
y=ang2-(dist-500)*k;
z=ang3-(dist-500)*k;
w=ang4-(dist-500)*k;
servo1.write(v);
servo2.write(y);
servo3.write(z);
servo2.write(w);
}
void angle5(int dist)
{
v=ang1-(dist-3500)*k;
y=ang2-(dist-3500)*k;
z=ang3-(dist-3500)*k;
w=ang4-(dist-3500)*k;
servo1.write(v);
servo2.write(y);
servo3.write(z);
servo4.write(w);
}
void angle6(int dist)
{
v=(ang1-90)+ (dist-4250)*k;
y=(ang2-90)+ (dist-4250)*k;
z=(ang3-90)+ (dist-4250)*k;
w=(ang4-90)+ (dist-4250)*k;
servo1.write(v);
servo2.write(y);
servo3.write(z);
servo4.write(w);
}
void angle7(int dist)
{
v=ang1+(dist-5000)*k;
y=ang2+(dist-5000)*k;
z=ang3+(dist-5000)*k;
w=ang3+(dist-5000)*k;
servo1.write(v);
servo2.write(y);
servo3.write(z);
servo4.write(w);
}
int main(){
pinMode(echo,0);
pinMode(trig,1);
for(i=1;i<=5750;i=i++)
{
servo1.attach(11);
servo2.attach(12);
servo3.attach(5);
servo4.attach(3);
/*
digitalWrite(trig,LOW);
delayMicroseconds(2);
digitalWrite(trig,HIGH);
delayMicroseconds(10);
digitalWrite(trig,LOW);
//Distance is calculated in cm using US sensor
int dur,distance;
dur=pUlsein(echo,HIGH);
distance=0.034*dur/2;
dist=distance*100; // Distance is converted in mm and stored in 'val' variable
//According to the distance,we will decide in which quarter of the sine wave we should enter.
*/
if((500<=dist)&&(dist<=1250))
{
angle4(dist);
}
else if((1250<dist)&&(dist<=2000))
{
angle1(dist);
}
else if((2000<dist)&&(dist<=2750))
{
angle2(dist);
}
else if((2750<dist)&&(dist<=3500))
{
angle3(dist);
}
else if((3500<dist)&&(dist<=4250))
{
angle5(dist);
}
else if((4250<dist)&&(dist<=5000))
{
angle6(dist);
}
else if((5000<dist)&&(dist<=5750))
{
angle7(dist);
}
}
}