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
|
/*
* Basic Servo library test program.
*
* Setup:
*
* - Connect a potentiometer to POT_PIN (default pin 15)
* - Connect an oscilloscope to SERVO_PIN1 (default pin 5) and
* SERVO_PIN2 (default pin 6).
* - Connect a serial monitor to SerialUSB
*
* The potentiometer controls the target angle for each of two Servo
* objects, one with angles in [-90, 90], and another in [0, 180].
* Servo pulse width range is [1000, 2000].
*
* Serial2 will tell you what inputs it's giving to each servo object,
* and some information it gets back. Pressing the button
* detaches/reattaches the Servo objects.
*
* Tests you should perform:
*
* - Check calculated pulse widths for each servo's target angle
* - Check that calculated pulse widths match actual pulse widths
* - Check that the period of the pulse train is roughly 20 ms
* - Check that the pulses stop when detached, and resume when reattached
* - Check that Servo::write() and Servo::read() round-trip properly
*
* This file is released into the public domain.
*/
#include <stdio.h>
#include <wirish/wirish.h>
#include "libraries/Servo/Servo.h"
#define POT_PIN 15
#define MIN_PW 1000
#define MAX_PW 2000
#define SERVO_PIN1 5
#define MIN_ANGLE1 0
#define MAX_ANGLE1 180
#define SERVO_PIN2 6
#define MIN_ANGLE2 (-90)
#define MAX_ANGLE2 90
Servo servo1;
Servo servo2;
#define BUF_SIZE 100
char buf[BUF_SIZE];
#define print_buf(fmt, ...) do { \
snprintf(buf, BUF_SIZE, fmt, __VA_ARGS__); \
Serial2.println(buf); } while (0)
int averageAnalogReads(int);
void attach();
void detach();
void setup() {
pinMode(POT_PIN, INPUT_ANALOG);
pinMode(BOARD_BUTTON_PIN, INPUT);
pinMode(BOARD_LED_PIN, OUTPUT);
Serial2.begin(9600);
servo1.attach(SERVO_PIN1, MIN_PW, MAX_PW, MIN_ANGLE1, MAX_ANGLE1);
servo2.attach(SERVO_PIN2, MIN_PW, MAX_PW, MIN_ANGLE2, MAX_ANGLE2);
ASSERT(servo1.attachedPin() == SERVO_PIN1);
ASSERT(servo2.attachedPin() == SERVO_PIN2);
}
void loop() {
delay(250);
toggleLED();
if (isButtonPressed()) {
if (servo1.attached()) detach();
else attach();
}
if (!servo1.attached()) return;
int32 average = averageAnalogReads(250);
int16 angle1 = (int16)map(average, 0, 4095, MIN_ANGLE1, MAX_ANGLE1);
int16 angle2 = (int16)map(average, 0, 4095, MIN_ANGLE2, MAX_ANGLE2);
print_buf("pot reading = %d, angle 1 = %d, angle 2 = %d.",
average, angle1, angle2);
servo1.write(angle1);
servo2.write(angle2);
int16 read1 = servo1.read();
int16 read2 = servo2.read();
print_buf("write/read angle 1: %d/%d, angle 2: %d/%d",
angle1, read1, angle2, read2);
ASSERT(abs(angle1 - read1) <= 1);
ASSERT(abs(angle2 - read2) <= 1);
print_buf("pulse width 1: %d, pulse width 2: %d",
servo1.readMicroseconds(), servo2.readMicroseconds());
Serial2.println("\n--------------------------\n");
}
int32 averageAnalogReads(int n) {
uint64 total = 0;
for (int i = 0; i < n; i++) {
total += analogRead(POT_PIN);
}
return (int32)(total / n);
}
void attach() {
Serial2.println("attaching");
servo1.attach(SERVO_PIN1);
servo2.attach(SERVO_PIN2);
ASSERT(servo1.attachedPin() == SERVO_PIN1);
ASSERT(servo2.attachedPin() == SERVO_PIN2);
}
void detach() {
Serial2.println("detaching");
servo1.detach();
servo2.detach();
ASSERT(!servo1.attached());
ASSERT(!servo2.attached());
}
// Force init to be called *first*, i.e. before static object allocation.
// Otherwise, statically allocated objects that need libmaple may fail.
__attribute__((constructor)) void premain() {
init();
}
int main(void) {
setup();
while (true) {
loop();
}
return 0;
}
|
