![](images/followtv_1.jpg)
- // FollowTV
- // pkvi
- #include "Servo.h"
- const int atpin = 2;
- const int aepin = 3;
- const int btpin = 4;
- const int bepin = 5;
- const int ctpin = 6;
- const int cepin = 7;
- // For Cycle
- const int num = 15;
- int i = 0;
- // Limit Distance
- const int limit = 5500;
- int hold = 500;
- long adur, atot, afix;
- long bdur, btot, bfix;
- long cdur, ctot, cfix;
- Servo lfs; // Left Front Speaker
- const int lfsout = 69;
- const int lfsin = 92;
- Servo mon; // Monitor
- const int monl = 145;
- const int monc = 91;
- const int monr = 45;
- Servo rfs; // Right Front Speaker
- const int rfsin = 63;
- const int rfsout = 78;
- void setup() {
- Serial.begin(9600);
- pinMode(atpin, OUTPUT);
- pinMode(aepin, INPUT);
- pinMode(btpin, OUTPUT);
- pinMode(bepin, INPUT);
- pinMode(ctpin, OUTPUT);
- pinMode(cepin, INPUT);
- lfs.attach(8);
- mon.attach(9);
- rfs.attach(10);
- digitalWrite(atpin, LOW);
- digitalWrite(btpin, LOW);
- digitalWrite(ctpin, LOW);
- lfs.write(lfsin);
- mon.write(monc);
- rfs.write(rfsin);
- }
- void loop() {
- atot = 0;
- for (i=0; i <= num; i++){
- digitalWrite(atpin, HIGH);
- delayMicroseconds(10);
- digitalWrite(atpin, LOW);
- adur = pulseIn(aepin, HIGH);
- if (adur > limit) {
- adur = limit;
- }
- atot = atot + adur;
- }
- afix = atot / num;
- btot = 0;
- for (i=0; i <= num; i++){
- digitalWrite(btpin, HIGH);
- delayMicroseconds(10);
- digitalWrite(btpin, LOW);
- bdur = pulseIn(bepin, HIGH);
- if (bdur > limit) {
- bdur = limit;
- }
- btot = btot + bdur;
- }
- bfix = btot / num;
- ctot = 0;
- for (i=0; i <= num; i++){
- digitalWrite(ctpin, HIGH);
- delayMicroseconds(10);
- digitalWrite(ctpin, LOW);
- cdur = pulseIn(cepin, HIGH);
- if (cdur > limit) {
- cdur = limit;
- }
- ctot = ctot + cdur;
- }
- cfix = ctot / num;
- Serial.print(afix);
- Serial.print(" ");
- Serial.print(bfix);
- Serial.print(" ");
- Serial.println(cfix);
- if (afix < bfix && afix < cfix) {
- // Position 2
- lfs.write(lfsout);
- mon.write(monl);
- rfs.write(rfsin);
- }
- else if (bfix < afix && bfix < cfix) {
- // Position 1 and 3
- lfs.write(lfsin);
- mon.write(monc);
- rfs.write(rfsin);
- }
- else if (cfix < afix && cfix < bfix) {
- // Position 4
- lfs.write(lfsin);
- mon.write(monr);
- rfs.write(rfsout);
- }
- else if (afix == limit && bfix == limit && cfix == limit) {
- // Position Open
- lfs.write(lfsout);
- mon.write(monc);
- rfs.write(rfsout);
- }
- delay(hold);
- }