/
CompassControl.ino
49 lines (38 loc) · 1.37 KB
/
CompassControl.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
/*
Copyright (c) 2013 Arduino LLC. All right reserved.
This library is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either
version 2.1 of the License, or (at your option) any later version.
This library is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
Lesser General Public License for more details.
You should have received a copy of the GNU Lesser General Public
License along with this library; if not, write to the Free Software
Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
*/
void pivotTo(int target){
while(1){
//compass_update();
int currentAngle = wp_heading;
int diff = target - currentAngle;
//telem << "Compass Control: " << endl;
//telem << "\t" << currentAngle << ", " << target << ", " << diff << endl;
if(diff > 0) {
motorSpeed_right = turnSpeed;
motorSpeed_left = turnSpeed;
mRight();//right
smartDelay(50);
} else {
motorSpeed_right = turnSpeed;
motorSpeed_left = turnSpeed;
mLeft();//left
smartDelay(50);
}
if(abs(diff) < HEADING_TOLERANCE){
mStop();
return;
}
}
}