blob: 900ce6aef2f1b543ca2107fc07ce328420b2ff2c (
plain) (
blame)
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
|
#include <Servo.h>
// Note: the library disables analogWrite on pins 9 and 10
#define INPUT_PIN A0
#define SERVO_1_PIN 3
#define SERVO_2_PIN 5
#define SERVO_3_PIN 6
#define SERVO_4_PIN 9
#define SERVO_5_PIN 10
Servo servo_1;
Servo servo_2;
Servo servo_3;
Servo servo_4;
Servo servo_5;
// Potentiometer value, will be the input value from the main controller
int potValue;
void zeroOutServos() {
servo_1.write(0);
servo_2.write(0);
servo_3.write(0);
servo_4.write(0);
servo_5.write(0);
}
void setup()
{
pinMode(INPUT_PIN, INPUT);
pinMode(SERVO_1_PIN, OUTPUT);
servo_1.attach(SERVO_1_PIN);
pinMode(SERVO_2_PIN, OUTPUT);
servo_2.attach(SERVO_2_PIN);
pinMode(SERVO_3_PIN, OUTPUT);
servo_3.attach(SERVO_3_PIN);
pinMode(SERVO_4_PIN, OUTPUT);
servo_4.attach(SERVO_4_PIN);
pinMode(SERVO_5_PIN, OUTPUT);
servo_5.attach(SERVO_5_PIN);
delay(2000);
zeroOutServos();
}
void loop()
{
potValue = analogRead(INPUT_PIN) >> 2;
// For demo purposes, continuously rotates servo
dispensePill((potValue * 5 / 255) + 1);
delay(100);
}
void dispensePill(int servoIndex) {
switch(servoIndex) {
case 1:
servo_1.write((servo_1.read() == 180)?0:180);
break;
case 2:
servo_2.write((servo_2.read() == 180)?0:180);
break;
case 3:
servo_3.write((servo_3.read() == 180)?0:180);
break;
case 4:
servo_4.write((servo_4.read() == 180)?0:180);
break;
case 5:
servo_5.write((servo_5.read() == 180)?0:180);
break;
}
delay(1500);
}
|
