-
Notifications
You must be signed in to change notification settings - Fork 0
/
SinusSquareWithServo.ino
88 lines (70 loc) · 1.62 KB
/
SinusSquareWithServo.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
/*
* Smooth servo rotation using a sinus square function.
* more infos: http://letsmakerobots.com/node/31697
* created by NilsB
*/
#include <MsTimer2.h>
#include <Servo.h>
const int timerInMillies = 20;
const int countSinusSquareLookupTableEntries = 100;
const int sinusSquareLookupTable[] = {
0,0,1,1,2,2,3,4,4,5,6,
7,9,10,11,13,14,16,17,19,21,22,
24,26,28,30,33,35,37,39,42,44,47,
49,52,54,57,60,62,65,68,70,73,76,
79,82,84,87,90,93,96,98,101,104,107,
110,112,115,118,120,123,126,128,131,133,136,
138,141,143,145,147,150,152,154,156,158,159,
161,163,164,166,167,169,170,171,173,174,175,
176,176,177,178,178,179,179,180,180,180,180
};
int currentValueIndex = 0;
const int movmentIndicatorPin = 13;
const Servo headServo;
void move()
{
moveServoTo(angle());
indicateMovement();
incrementOrStop();
}
void moveServoTo(int angle){
Serial.println(angle);
headServo.write(angle);
}
int angle(){
return sinusSquareLookupTable[currentValueIndex];
}
void indicateMovement()
{
static boolean output = HIGH;
digitalWrite(movmentIndicatorPin, output);
output = !output;
}
void incrementOrStop(){
currentValueIndex++;
if(currentValueIndex == countSinusSquareLookupTableEntries - 1){
stopMove();
}
}
void stopMove(){
MsTimer2::stop();
headServo.detach();
}
void setupMovementIndicator(){
Serial.begin(9600);
pinMode(movmentIndicatorPin, OUTPUT);
}
void setupTimer2(){
MsTimer2::set(timerInMillies, move);
MsTimer2::start();
}
void setupServo(){
headServo.attach(6);
headServo.write(0);
}
void setup(){
setupMovementIndicator();
setupServo();
setupTimer2();
}
void loop(){;}