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
|
#include <Servo.h>
Servo drive;
int drive_pin = 9;
int orient_pin = 0;
int orient = 0;
int old_orient = -1;
int want_orient = 0;
int drive_signal = 0;
int old_drive_signal = 0;
int diff = 0;
int default_diff;
int strength = 0;
int slower = 0;
int ledPin = 12;
void setup()
{
drive.attach(drive_pin);
pinMode(ledPin, OUTPUT);
pinMode(13, OUTPUT);
want_orient = analogRead(orient_pin);
strength = 0; // 50
default_diff = 5;
orient = analogRead(orient_pin);
old_orient = orient;
//Serial.begin(9600);
}
void loop()
{
slower = slower+2;
if(slower > 30) {
slower = 0;
want_orient = want_orient + 1;
}
if(want_orient < 0) { want_orient = 1023; }
if(want_orient > 1023) { want_orient = 0; }
// crude ADC smoothing
orient = (old_orient + analogRead(orient_pin))/2;
old_orient = orient;
diff = find_diff(orient, want_orient);
//if(orient < want_orient) { digitalWrite(ledPin, HIGH); }
//else { digitalWrite(ledPin, LOW); }
//want_orient = 100;
if(diff > 10) {
digitalWrite(ledPin, LOW);
strength = strength +1;
} else if (diff < -10) {
digitalWrite(ledPin, LOW);
strength = strength -1;
} else {
digitalWrite(ledPin, HIGH);
}
strength = constrain(strength, 0, 100);
drive_signal = 91 - strength/10 - constrain(diff,-20,20)/2;
// This is to try and get around the weird gap
if(want_orient < 10 || want_orient > 1013) {
drive_signal = old_drive_signal;
// } else if(orient < 10 || orient > 1013) {
// drive_signal = old_drive_signal;
} else {
old_drive_signal = drive_signal;
}
drive.write(drive_signal); // had to choose some polarity...
//Serial.println(diff, DEC);
delay(15);
}
// Express the difference between a to b, accounting for the wrap around
// a and b are both 0-1023
int find_diff(int a, int b) {
int c;
c = b - a;
if(c < - 511) {
c = c + 1023;
} else if (c > 512) {
c = c - 1023;
}
if(abs(c) > 100) {
digitalWrite(13, HIGH);
} else {
digitalWrite(13, LOW);
}
return c;
}
|