blob: b625714530ba4bb58e4dc860d7e118b2ba5c71f9 (
plain)
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
|
#include <Servo.h>
Servo drive;
int drive_pin = 9;
int orient_pin = 0;
int orient = 0;
int want_orient = 0;
int diff = 0;
int strength = 0;
int slower = 0;
int ledPin = 13;
void setup()
{
drive.attach(drive_pin);
pinMode(ledPin, OUTPUT);
}
void loop()
{
want_orient = want_orient + 1;
if(want_orient > 1023) { want_orient = want_orient - 1024; }
orient = analogRead(orient_pin);
want_orient = 100;
if(orient - want_orient > 50) {
digitalWrite(ledPin, HIGH);
strength = strength +1;
} else if (orient - want_orient < -50) {
digitalWrite(ledPin, HIGH);
strength = strength -1;
} else {
digitalWrite(ledPin, LOW);
}
strength = constrain(strength, -40, 40);
drive.write(91 + strength/10);
/*
//diff = want_orient - orient;
//if(abs(diff) > 511) diff = (diff/abs(diff))*(1024-diff);
diff = 0;
diff = diff / (511/89);
digitalWrite(ledPin, LOW);
if(abs(diff) < 20) { strength = 90; digitalWrite(ledPin, HIGH); }
else if (abs(diff) > 90) strength = (diff/abs(diff)) * 89 + 90;
else strength = 90 + diff;
if(abs(want_orient - orient) < 50) digitalWrite(ledPin, HIGH);
drive.write(strength);
*/
delay(15);
}
|