Easy Light Tracking Device

Light tracking made easy with Arduino

Jul 2, 2021

•

1344 views

•

0 respects

environmental sensing

lights

tracking

Components and supplies

Arduino Nano R3

Resistor 10k ohm

SG90 Micro-servo motor

Photo resistor

Jumper wires (generic)

Tools and machines

3D Printer (generic)

Soldering iron (generic)

Project description

Code

Source code

arduino

This is the source code for this tutorial

1/**
2   Author: Kevin Chen AKA Stonez56 
3   My Blog to see more tutorials:  https://stonez56.blogspot.com
4   YouTube channel:https://www.youtube.com/channel/UCLdNauvVOrss3fF8LJXrtAw
5  
6    Program function flow
7   1. Reading four light sensor values
8   2.  Average top two(Top left, Top right), bottom two(Bottom left, Bottom right)
9   3. Average left two(Top left, Bottom left), right two (Top right, Bottom right)
10   4. Compare 4 averaged value and make servo mave toward the biggest number side
11   
12   v0 Light traking Gimbal base to get readings
13   v1 Write servo code  in and move servo accordingly
14 
15                    |
16         Top left   |     Top right
17   -----------------+----------------------
18    　Bottom  left   |  Bottom right
19                    |
20*/
21#include <Servo.h>
22Servo  servo1_x;
23Servo servo2_y;
24 
25//Light Sensor Pin definitions
26const uint8_t  light_top_left_PIN = A6;
27const uint8_t  light_bottom_left_PIN = A5;
28const  uint8_t  light_top_rigth_PIN = A2;
29const uint8_t  light_bottom_right_PIN = A1;
30//Potentiometer  pin
31const  int potPIN = A0;
32//Servo pins
33const int servo1_x_pin = 2;
34const  int servo2_y_pin = 4;
35 
36 
37//User define variables
38// Gimbal movement  tolerance 50~255
39byte gimbal_movement_tolerance = 100;
40//Photo sensor max  reading times for average (for more accuracy)
41byte max_reading = 10;
42// define  original servo angle
43uint8_t originalAngle = 92;
44uint8_t x_last = originalAngle;  // X last postion
45uint8_t y_last = originalAngle; // Y last postion
46uint8_t  moveSpeed = 10;  //How fast show this Servo move
47uint8_t maxSpeed = 50; //Max  speed
48uint8_t minSpeed = 1;
49uint8_t y_minAngle = 1;  //Mimum angle
50uint8_t  y_maxAngle = 180;  //Maximum angle
51uint8_t x_minAngle = 90;  //Mimum angle
52uint8_t  x_maxAngle = 180;  //Maximum angle
53 
54void setup() {
55  Serial.begin(57600);
56  
57  pinMode(light_top_left_PIN, INPUT);
58  pinMode(light_bottom_left_PIN, INPUT);
59  pinMode(light_top_rigth_PIN, INPUT);
60  pinMode(light_bottom_right_PIN, INPUT);
61  
62  servo1_x.attach(servo1_x_pin);
63  servo1_x.write(originalAngle); //move  servo to defined angle
64  servo2_y.attach(servo2_y_pin);
65  servo2_y.write(originalAngle);  //move servo to defined angle
66 
67  pinMode(LED_BUILTIN, OUTPUT);
68  digitalWrite(LED_BUILTIN,  LOW);
69 
70}
71 
72void loop() {
73  char moveTowards = ' ';
74 
75 
76  int potValue = analogRead(potPIN);
77  Serial.print(F("potValue: "));
78  Serial.println(potValue);
79  moveSpeed = map(potValue, 0, 500, minSpeed, maxSpeed);
80  Serial.print(F("moveSpeed:  "));
81  Serial.println(moveSpeed);
82  moveTowards = getFacingToward(); //find  out which side to move
83 
84 
85//  Serial.print(F("moveTowards: "));
86//  Serial.println(moveTowards);
87  moveServo(moveTowards);
88 
89  delay(80);
90}
91  
92void moveServo(char moveTowards) {
93 
94 
95  //previous position were stored  in x_last, y_last
96  switch (moveTowards) {
97    case 'T': //Move towards top
98      if (x_last - moveSpeed < x_minAngle) {
99        x_last = x_minAngle;
100        servo1_x.write(x_last);
101      } else {
102        x_last -= moveSpeed;
103        servo1_x.write(x_last);
104      }
105      break;
106    case 'B': //Move  towards bottom
107      if (x_last + moveSpeed > x_maxAngle) {
108        x_last  = x_maxAngle;
109        servo1_x.write(x_last);
110      } else {
111        x_last  += moveSpeed;
112        servo1_x.write(x_last);
113      }
114      break;
115    case 'L': //Move towards left
116      if (y_last - moveSpeed < y_minAngle)  {
117        y_last = y_minAngle;
118        servo2_y.write(y_last);
119      }  else {
120        y_last -= moveSpeed;
121        servo2_y.write(y_last);
122      }
123      break;
124    case 'R':
125      if (y_last + moveSpeed > y_maxAngle) {
126        y_last = y_maxAngle;
127        servo2_y.write(y_last);
128      } else  {
129        y_last += moveSpeed;
130        servo2_y.write(y_last);
131      }
132      break;
133    default:
134      //Don't move servo
135      break;
136  }
137  
138}
139 
140/**
141   This fuction get reading max_reading times and return average
142   if it sees an 0, it will skip it
143*/
144int averageReading(int PIN, byte max_reading)  {
145  int total = 0;
146  int current = 0;
147 
148  for (byte i = 0; i <= max_reading;  i++) {
149    current = analogRead(PIN);
150    if (current == 0) {
151      current  = analogRead(PIN);
152    }
153    total += current;
154  }
155 
156  return total  / (max_reading);
157}
158 
159/**
160 
161 
162   Parameters: N/A
163   Return:  char; T, B, R, L to indicate the moving directoin
164                       '-'  means not moving at all
165*/
166char getFacingToward() {
167  //1. Read each pin  max_reading times
168 
169  int top_left = averageReading(light_top_left_PIN, max_reading);
170  int bottom_left = averageReading(light_bottom_left_PIN, max_reading);
171  int  top_right = averageReading(light_top_rigth_PIN, max_reading);
172  int bottom_right  = averageReading(light_bottom_right_PIN, max_reading * 3);
173 
174  //Show photo  sensor readings...
175  Serial.print(F("Top left-A6: "));
176  Serial.println(top_left);
177  Serial.print(F("Bottom left-A5: "));
178  Serial.println(bottom_left);
179  Serial.print(F("Top  right-A2: "));
180  Serial.println(top_right);
181  Serial.print(F("Bottom rightA1:  "));
182  Serial.println(bottom_right);
183 
184  //2. Get max value sides(two  averaged, see above)
185  byte go_direction[4] = {0, 0, 0, 0};
186  int toward[4]  = {0, 0, 0, 0}; //Top, Bottom, Left, Right
187  toward[0] = (top_left + top_right)  / 2 ;
188  toward[1] = (bottom_left + bottom_right) / 2 ;
189  toward[2] = (top_left  + bottom_left) / 2;
190  toward[3] = (top_right + bottom_right) / 2;
191 
192  //3.  Add all side and average,
193  //    if each side is within the tolerance then don't  move Gimbal
194  //    average = toward[0] +...toward[3]
195  //    average - toward[0]  .... toward[3], if all within gimbal_movement_tolerance, then no move
196  int total_toward  = 0;
197  total_toward += toward[0];
198  total_toward += toward[1];
199  total_toward  += toward[2];
200  total_toward += toward[3];
201  //get average
202  int total_average  = total_toward / 4; //4 sides
203  Serial.print(F("total_average:"));
204  Serial.println(total_average);
205  //    if each side is within the tolerance then don't move Gimbal
206  boolean  shouldMove = false;
207  //else move the gimbal
208  if (total_average - toward[0]  > gimbal_movement_tolerance) shouldMove = true;
209  if (total_average - toward[1]  > gimbal_movement_tolerance) shouldMove = true;
210  if (total_average - toward[2]  > gimbal_movement_tolerance) shouldMove = true;
211  if (total_average - toward[3]  > gimbal_movement_tolerance) shouldMove = true;
212 
213  Serial.print(F("toward  0 TOP : "));
214  Serial.println(toward[0]);
215  Serial.print(F("toward 1 BOTTOM:  "));
216  Serial.println(toward[1]);
217  Serial.print(F("toward 2 LEFT: "));
218  Serial.println(toward[2]);
219  Serial.print(F("toward 3 RIGHT: "));
220  Serial.println(toward[3]);
221  
222  //Find the biggest number to decide which side to go,
223  // but if four  values are quite similar, send '-' back to indicate not moving
224  char facing  = ' ' ;
225  if (shouldMove) {
226    int max_ = 0;
227    if (toward[0] > max_)  {
228      max_ = toward[0];
229      facing = 'T';
230    }
231    if (toward[1]  > max_) {
232      max_ = toward[1];
233      facing = 'B';
234    }
235    if (toward[2]  > max_) {
236      max_ = toward[2];
237      facing = 'L';
238    }
239    if (toward[3]  > max_) {
240      max_ = toward[3];
241      facing = 'R';
242    }
243  } else  {
244    facing = '-'; //no need to move
245  }
246  //  Serial.print(F("shouldMove:  "));
247  //  Serial.println(shouldMove);
248  return facing;
249}

/**
   Author: Kevin Chen AKA Stonez56 
   My Blog to see more tutorials:  https://stonez56.blogspot.com
   YouTube channel:https://www.youtube.com/channel/UCLdNauvVOrss3fF8LJXrtAw
  
    Program function flow
   1. Reading four light sensor values
   2.  Average top two(Top left, Top right), bottom two(Bottom left, Bottom right)
   3. Average left two(Top left, Bottom left), right two (Top right, Bottom right)
   4. Compare 4 averaged value and make servo mave toward the biggest number side
   
   v0 Light traking Gimbal base to get readings
   v1 Write servo code  in and move servo accordingly
 
                    |
         Top left   |     Top right
   -----------------+----------------------
    　Bottom  left   |  Bottom right
                    |
*/
#include <Servo.h>
Servo  servo1_x;
Servo servo2_y;
 
//Light Sensor Pin definitions
const uint8_t  light_top_left_PIN = A6;
const uint8_t  light_bottom_left_PIN = A5;
const  uint8_t  light_top_rigth_PIN = A2;
const uint8_t  light_bottom_right_PIN = A1;
//Potentiometer  pin
const  int potPIN = A0;
//Servo pins
const int servo1_x_pin = 2;
const  int servo2_y_pin = 4;
 
 
//User define variables
// Gimbal movement  tolerance 50~255
byte gimbal_movement_tolerance = 100;
//Photo sensor max  reading times for average (for more accuracy)
byte max_reading = 10;
// define  original servo angle
uint8_t originalAngle = 92;
uint8_t x_last = originalAngle;  // X last postion
uint8_t y_last = originalAngle; // Y last postion
uint8_t  moveSpeed = 10;  //How fast show this Servo move
uint8_t maxSpeed = 50; //Max  speed
uint8_t minSpeed = 1;
uint8_t y_minAngle = 1;  //Mimum angle
uint8_t  y_maxAngle = 180;  //Maximum angle
uint8_t x_minAngle = 90;  //Mimum angle
uint8_t  x_maxAngle = 180;  //Maximum angle
 
void setup() {
  Serial.begin(57600);
  
  pinMode(light_top_left_PIN, INPUT);
  pinMode(light_bottom_left_PIN, INPUT);
  pinMode(light_top_rigth_PIN, INPUT);
  pinMode(light_bottom_right_PIN, INPUT);
  
  servo1_x.attach(servo1_x_pin);
  servo1_x.write(originalAngle); //move  servo to defined angle
  servo2_y.attach(servo2_y_pin);
  servo2_y.write(originalAngle);  //move servo to defined angle
 
  pinMode(LED_BUILTIN, OUTPUT);
  digitalWrite(LED_BUILTIN,  LOW);
 
}
 
void loop() {
  char moveTowards = ' ';
 
 
  int potValue = analogRead(potPIN);
  Serial.print(F("potValue: "));
  Serial.println(potValue);
  moveSpeed = map(potValue, 0, 500, minSpeed, maxSpeed);
  Serial.print(F("moveSpeed:  "));
  Serial.println(moveSpeed);
  moveTowards = getFacingToward(); //find  out which side to move
 
 
//  Serial.print(F("moveTowards: "));
//  Serial.println(moveTowards);
  moveServo(moveTowards);
 
  delay(80);
}
  
void moveServo(char moveTowards) {
 
 
  //previous position were stored  in x_last, y_last
  switch (moveTowards) {
    case 'T': //Move towards top
      if (x_last - moveSpeed < x_minAngle) {
        x_last = x_minAngle;
        servo1_x.write(x_last);
      } else {
        x_last -= moveSpeed;
        servo1_x.write(x_last);
      }
      break;
    case 'B': //Move  towards bottom
      if (x_last + moveSpeed > x_maxAngle) {
        x_last  = x_maxAngle;
        servo1_x.write(x_last);
      } else {
        x_last  += moveSpeed;
        servo1_x.write(x_last);
      }
      break;
    case 'L': //Move towards left
      if (y_last - moveSpeed < y_minAngle)  {
        y_last = y_minAngle;
        servo2_y.write(y_last);
      }  else {
        y_last -= moveSpeed;
        servo2_y.write(y_last);
      }
      break;
    case 'R':
      if (y_last + moveSpeed > y_maxAngle) {
        y_last = y_maxAngle;
        servo2_y.write(y_last);
      } else  {
        y_last += moveSpeed;
        servo2_y.write(y_last);
      }
      break;
    default:
      //Don't move servo
      break;
  }
  
}
 
/**
   This fuction get reading max_reading times and return average
   if it sees an 0, it will skip it
*/
int averageReading(int PIN, byte max_reading)  {
  int total = 0;
  int current = 0;
 
  for (byte i = 0; i <= max_reading;  i++) {
    current = analogRead(PIN);
    if (current == 0) {
      current  = analogRead(PIN);
    }
    total += current;
  }
 
  return total  / (max_reading);
}
 
/**
 
 
   Parameters: N/A
   Return:  char; T, B, R, L to indicate the moving directoin
                       '-'  means not moving at all
*/
char getFacingToward() {
  //1. Read each pin  max_reading times
 
  int top_left = averageReading(light_top_left_PIN, max_reading);
  int bottom_left = averageReading(light_bottom_left_PIN, max_reading);
  int  top_right = averageReading(light_top_rigth_PIN, max_reading);
  int bottom_right  = averageReading(light_bottom_right_PIN, max_reading * 3);
 
  //Show photo  sensor readings...
  Serial.print(F("Top left-A6: "));
  Serial.println(top_left);
  Serial.print(F("Bottom left-A5: "));
  Serial.println(bottom_left);
  Serial.print(F("Top  right-A2: "));
  Serial.println(top_right);
  Serial.print(F("Bottom rightA1:  "));
  Serial.println(bottom_right);
 
  //2. Get max value sides(two  averaged, see above)
  byte go_direction[4] = {0, 0, 0, 0};
  int toward[4]  = {0, 0, 0, 0}; //Top, Bottom, Left, Right
  toward[0] = (top_left + top_right)  / 2 ;
  toward[1] = (bottom_left + bottom_right) / 2 ;
  toward[2] = (top_left  + bottom_left) / 2;
  toward[3] = (top_right + bottom_right) / 2;
 
  //3.  Add all side and average,
  //    if each side is within the tolerance then don't  move Gimbal
  //    average = toward[0] +...toward[3]
  //    average - toward[0]  .... toward[3], if all within gimbal_movement_tolerance, then no move
  int total_toward  = 0;
  total_toward += toward[0];
  total_toward += toward[1];
  total_toward  += toward[2];
  total_toward += toward[3];
  //get average
  int total_average  = total_toward / 4; //4 sides
  Serial.print(F("total_average:"));
  Serial.println(total_average);
  //    if each side is within the tolerance then don't move Gimbal
  boolean  shouldMove = false;
  //else move the gimbal
  if (total_average - toward[0]  > gimbal_movement_tolerance) shouldMove = true;
  if (total_average - toward[1]  > gimbal_movement_tolerance) shouldMove = true;
  if (total_average - toward[2]  > gimbal_movement_tolerance) shouldMove = true;
  if (total_average - toward[3]  > gimbal_movement_tolerance) shouldMove = true;
 
  Serial.print(F("toward  0 TOP : "));
  Serial.println(toward[0]);
  Serial.print(F("toward 1 BOTTOM:  "));
  Serial.println(toward[1]);
  Serial.print(F("toward 2 LEFT: "));
  Serial.println(toward[2]);
  Serial.print(F("toward 3 RIGHT: "));
  Serial.println(toward[3]);
  
  //Find the biggest number to decide which side to go,
  // but if four  values are quite similar, send '-' back to indicate not moving
  char facing  = ' ' ;
  if (shouldMove) {
    int max_ = 0;
    if (toward[0] > max_)  {
      max_ = toward[0];
      facing = 'T';
    }
    if (toward[1]  > max_) {
      max_ = toward[1];
      facing = 'B';
    }
    if (toward[2]  > max_) {
      max_ = toward[2];
      facing = 'L';
    }
    if (toward[3]  > max_) {
      max_ = toward[3];
      facing = 'R';
    }
  } else  {
    facing = '-'; //no need to move
  }
  //  Serial.print(F("shouldMove:  "));
  //  Serial.println(shouldMove);
  return facing;
}

Source code

arduino

This is the source code for this tutorial

1/**
2   Author: Kevin Chen AKA Stonez56 
3   My Blog to see more tutorials:
4  https://stonez56.blogspot.com
5   YouTube channel:https://www.youtube.com/channel/UCLdNauvVOrss3fF8LJXrtAw
6
7  
8    Program function flow
9   1. Reading four light sensor values
10   2.
11  Average top two(Top left, Top right), bottom two(Bottom left, Bottom right)
12
13   3. Average left two(Top left, Bottom left), right two (Top right, Bottom right)
14
15   4. Compare 4 averaged value and make servo mave toward the biggest number side
16
17   
18   v0 Light traking Gimbal base to get readings
19   v1 Write servo code
20  in and move servo accordingly
21 
22                    |
23         Top left
24   |     Top right
25   -----------------+----------------------
26    　Bottom
27  left   |  Bottom right
28                    |
29*/
30#include <Servo.h>
31Servo
32  servo1_x;
33Servo servo2_y;
34 
35//Light Sensor Pin definitions
36const uint8_t
37  light_top_left_PIN = A6;
38const uint8_t  light_bottom_left_PIN = A5;
39const
40  uint8_t  light_top_rigth_PIN = A2;
41const uint8_t  light_bottom_right_PIN = A1;
42//Potentiometer
43  pin
44const  int potPIN = A0;
45//Servo pins
46const int servo1_x_pin = 2;
47const
48  int servo2_y_pin = 4;
49 
50 
51//User define variables
52// Gimbal movement
53  tolerance 50~255
54byte gimbal_movement_tolerance = 100;
55//Photo sensor max
56  reading times for average (for more accuracy)
57byte max_reading = 10;
58// define
59  original servo angle
60uint8_t originalAngle = 92;
61uint8_t x_last = originalAngle;
62  // X last postion
63uint8_t y_last = originalAngle; // Y last postion
64uint8_t
65  moveSpeed = 10;  //How fast show this Servo move
66uint8_t maxSpeed = 50; //Max
67  speed
68uint8_t minSpeed = 1;
69uint8_t y_minAngle = 1;  //Mimum angle
70uint8_t
71  y_maxAngle = 180;  //Maximum angle
72uint8_t x_minAngle = 90;  //Mimum angle
73uint8_t
74  x_maxAngle = 180;  //Maximum angle
75 
76void setup() {
77  Serial.begin(57600);
78
79  
80  pinMode(light_top_left_PIN, INPUT);
81  pinMode(light_bottom_left_PIN, INPUT);
82
83  pinMode(light_top_rigth_PIN, INPUT);
84  pinMode(light_bottom_right_PIN, INPUT);
85
86  
87  servo1_x.attach(servo1_x_pin);
88  servo1_x.write(originalAngle); //move
89  servo to defined angle
90  servo2_y.attach(servo2_y_pin);
91  servo2_y.write(originalAngle);
92  //move servo to defined angle
93 
94  pinMode(LED_BUILTIN, OUTPUT);
95  digitalWrite(LED_BUILTIN,
96  LOW);
97 
98}
99 
100void loop() {
101  char moveTowards = ' ';
102 
103 
104
105  int potValue = analogRead(potPIN);
106  Serial.print(F("potValue: "));
107  Serial.println(potValue);
108
109  moveSpeed = map(potValue, 0, 500, minSpeed, maxSpeed);
110  Serial.print(F("moveSpeed:
111  "));
112  Serial.println(moveSpeed);
113  moveTowards = getFacingToward(); //find
114  out which side to move
115 
116 
117//  Serial.print(F("moveTowards: "));
118//
119  Serial.println(moveTowards);
120  moveServo(moveTowards);
121 
122  delay(80);
123}
124
125  
126void moveServo(char moveTowards) {
127 
128 
129  //previous position were stored
130  in x_last, y_last
131  switch (moveTowards) {
132    case 'T': //Move towards top
133
134      if (x_last - moveSpeed < x_minAngle) {
135        x_last = x_minAngle;
136
137        servo1_x.write(x_last);
138      } else {
139        x_last -= moveSpeed;
140
141        servo1_x.write(x_last);
142      }
143      break;
144    case 'B': //Move
145  towards bottom
146      if (x_last + moveSpeed > x_maxAngle) {
147        x_last
148  = x_maxAngle;
149        servo1_x.write(x_last);
150      } else {
151        x_last
152  += moveSpeed;
153        servo1_x.write(x_last);
154      }
155      break;
156
157    case 'L': //Move towards left
158      if (y_last - moveSpeed < y_minAngle)
159  {
160        y_last = y_minAngle;
161        servo2_y.write(y_last);
162      }
163  else {
164        y_last -= moveSpeed;
165        servo2_y.write(y_last);
166      }
167
168      break;
169    case 'R':
170      if (y_last + moveSpeed > y_maxAngle) {
171
172        y_last = y_maxAngle;
173        servo2_y.write(y_last);
174      } else
175  {
176        y_last += moveSpeed;
177        servo2_y.write(y_last);
178      }
179
180      break;
181    default:
182      //Don't move servo
183      break;
184  }
185
186  
187}
188 
189/**
190   This fuction get reading max_reading times and return average
191
192   if it sees an 0, it will skip it
193*/
194int averageReading(int PIN, byte max_reading)
195  {
196  int total = 0;
197  int current = 0;
198 
199  for (byte i = 0; i <= max_reading;
200  i++) {
201    current = analogRead(PIN);
202    if (current == 0) {
203      current
204  = analogRead(PIN);
205    }
206    total += current;
207  }
208 
209  return total
210  / (max_reading);
211}
212 
213/**
214 
215 
216   Parameters: N/A
217   Return:
218  char; T, B, R, L to indicate the moving directoin
219                       '-'
220  means not moving at all
221*/
222char getFacingToward() {
223  //1. Read each pin
224  max_reading times
225 
226  int top_left = averageReading(light_top_left_PIN, max_reading);
227
228  int bottom_left = averageReading(light_bottom_left_PIN, max_reading);
229  int
230  top_right = averageReading(light_top_rigth_PIN, max_reading);
231  int bottom_right
232  = averageReading(light_bottom_right_PIN, max_reading * 3);
233 
234  //Show photo
235  sensor readings...
236  Serial.print(F("Top left-A6: "));
237  Serial.println(top_left);
238
239  Serial.print(F("Bottom left-A5: "));
240  Serial.println(bottom_left);
241  Serial.print(F("Top
242  right-A2: "));
243  Serial.println(top_right);
244  Serial.print(F("Bottom rightA1:
245  "));
246  Serial.println(bottom_right);
247 
248  //2. Get max value sides(two
249  averaged, see above)
250  byte go_direction[4] = {0, 0, 0, 0};
251  int toward[4]
252  = {0, 0, 0, 0}; //Top, Bottom, Left, Right
253  toward[0] = (top_left + top_right)
254  / 2 ;
255  toward[1] = (bottom_left + bottom_right) / 2 ;
256  toward[2] = (top_left
257  + bottom_left) / 2;
258  toward[3] = (top_right + bottom_right) / 2;
259 
260  //3.
261  Add all side and average,
262  //    if each side is within the tolerance then don't
263  move Gimbal
264  //    average = toward[0] +...toward[3]
265  //    average - toward[0]
266  .... toward[3], if all within gimbal_movement_tolerance, then no move
267  int total_toward
268  = 0;
269  total_toward += toward[0];
270  total_toward += toward[1];
271  total_toward
272  += toward[2];
273  total_toward += toward[3];
274  //get average
275  int total_average
276  = total_toward / 4; //4 sides
277  Serial.print(F("total_average:"));
278  Serial.println(total_average);
279
280  //    if each side is within the tolerance then don't move Gimbal
281  boolean
282  shouldMove = false;
283  //else move the gimbal
284  if (total_average - toward[0]
285  > gimbal_movement_tolerance) shouldMove = true;
286  if (total_average - toward[1]
287  > gimbal_movement_tolerance) shouldMove = true;
288  if (total_average - toward[2]
289  > gimbal_movement_tolerance) shouldMove = true;
290  if (total_average - toward[3]
291  > gimbal_movement_tolerance) shouldMove = true;
292 
293  Serial.print(F("toward
294  0 TOP : "));
295  Serial.println(toward[0]);
296  Serial.print(F("toward 1 BOTTOM:
297  "));
298  Serial.println(toward[1]);
299  Serial.print(F("toward 2 LEFT: "));
300
301  Serial.println(toward[2]);
302  Serial.print(F("toward 3 RIGHT: "));
303  Serial.println(toward[3]);
304
305  
306  //Find the biggest number to decide which side to go,
307  // but if four
308  values are quite similar, send '-' back to indicate not moving
309  char facing
310  = ' ' ;
311  if (shouldMove) {
312    int max_ = 0;
313    if (toward[0] > max_)
314  {
315      max_ = toward[0];
316      facing = 'T';
317    }
318    if (toward[1]
319  > max_) {
320      max_ = toward[1];
321      facing = 'B';
322    }
323    if (toward[2]
324  > max_) {
325      max_ = toward[2];
326      facing = 'L';
327    }
328    if (toward[3]
329  > max_) {
330      max_ = toward[3];
331      facing = 'R';
332    }
333  } else
334  {
335    facing = '-'; //no need to move
336  }
337  //  Serial.print(F("shouldMove:
338  "));
339  //  Serial.println(shouldMove);
340  return facing;
341}

/**
   Author: Kevin Chen AKA Stonez56 
   My Blog to see more tutorials:
  https://stonez56.blogspot.com
   YouTube channel:https://www.youtube.com/channel/UCLdNauvVOrss3fF8LJXrtAw

  
    Program function flow
   1. Reading four light sensor values
   2.
  Average top two(Top left, Top right), bottom two(Bottom left, Bottom right)

   3. Average left two(Top left, Bottom left), right two (Top right, Bottom right)

   4. Compare 4 averaged value and make servo mave toward the biggest number side

   
   v0 Light traking Gimbal base to get readings
   v1 Write servo code
  in and move servo accordingly
 
                    |
         Top left
   |     Top right
   -----------------+----------------------
    　Bottom
  left   |  Bottom right
                    |
*/
#include <Servo.h>
Servo
  servo1_x;
Servo servo2_y;
 
//Light Sensor Pin definitions
const uint8_t
  light_top_left_PIN = A6;
const uint8_t  light_bottom_left_PIN = A5;
const
  uint8_t  light_top_rigth_PIN = A2;
const uint8_t  light_bottom_right_PIN = A1;
//Potentiometer
  pin
const  int potPIN = A0;
//Servo pins
const int servo1_x_pin = 2;
const
  int servo2_y_pin = 4;
 
 
//User define variables
// Gimbal movement
  tolerance 50~255
byte gimbal_movement_tolerance = 100;
//Photo sensor max
  reading times for average (for more accuracy)
byte max_reading = 10;
// define
  original servo angle
uint8_t originalAngle = 92;
uint8_t x_last = originalAngle;
  // X last postion
uint8_t y_last = originalAngle; // Y last postion
uint8_t
  moveSpeed = 10;  //How fast show this Servo move
uint8_t maxSpeed = 50; //Max
  speed
uint8_t minSpeed = 1;
uint8_t y_minAngle = 1;  //Mimum angle
uint8_t
  y_maxAngle = 180;  //Maximum angle
uint8_t x_minAngle = 90;  //Mimum angle
uint8_t
  x_maxAngle = 180;  //Maximum angle
 
void setup() {
  Serial.begin(57600);

  
  pinMode(light_top_left_PIN, INPUT);
  pinMode(light_bottom_left_PIN, INPUT);

  pinMode(light_top_rigth_PIN, INPUT);
  pinMode(light_bottom_right_PIN, INPUT);

  
  servo1_x.attach(servo1_x_pin);
  servo1_x.write(originalAngle); //move
  servo to defined angle
  servo2_y.attach(servo2_y_pin);
  servo2_y.write(originalAngle);
  //move servo to defined angle
 
  pinMode(LED_BUILTIN, OUTPUT);
  digitalWrite(LED_BUILTIN,
  LOW);
 
}
 
void loop() {
  char moveTowards = ' ';
 
 

  int potValue = analogRead(potPIN);
  Serial.print(F("potValue: "));
  Serial.println(potValue);

  moveSpeed = map(potValue, 0, 500, minSpeed, maxSpeed);
  Serial.print(F("moveSpeed:
  "));
  Serial.println(moveSpeed);
  moveTowards = getFacingToward(); //find
  out which side to move
 
 
//  Serial.print(F("moveTowards: "));
//
  Serial.println(moveTowards);
  moveServo(moveTowards);
 
  delay(80);
}

  
void moveServo(char moveTowards) {
 
 
  //previous position were stored
  in x_last, y_last
  switch (moveTowards) {
    case 'T': //Move towards top

      if (x_last - moveSpeed < x_minAngle) {
        x_last = x_minAngle;

        servo1_x.write(x_last);
      } else {
        x_last -= moveSpeed;

        servo1_x.write(x_last);
      }
      break;
    case 'B': //Move
  towards bottom
      if (x_last + moveSpeed > x_maxAngle) {
        x_last
  = x_maxAngle;
        servo1_x.write(x_last);
      } else {
        x_last
  += moveSpeed;
        servo1_x.write(x_last);
      }
      break;

    case 'L': //Move towards left
      if (y_last - moveSpeed < y_minAngle)
  {
        y_last = y_minAngle;
        servo2_y.write(y_last);
      }
  else {
        y_last -= moveSpeed;
        servo2_y.write(y_last);
      }

      break;
    case 'R':
      if (y_last + moveSpeed > y_maxAngle) {

        y_last = y_maxAngle;
        servo2_y.write(y_last);
      } else
  {
        y_last += moveSpeed;
        servo2_y.write(y_last);
      }

      break;
    default:
      //Don't move servo
      break;
  }

  
}
 
/**
   This fuction get reading max_reading times and return average

   if it sees an 0, it will skip it
*/
int averageReading(int PIN, byte max_reading)
  {
  int total = 0;
  int current = 0;
 
  for (byte i = 0; i <= max_reading;
  i++) {
    current = analogRead(PIN);
    if (current == 0) {
      current
  = analogRead(PIN);
    }
    total += current;
  }
 
  return total
  / (max_reading);
}
 
/**
 
 
   Parameters: N/A
   Return:
  char; T, B, R, L to indicate the moving directoin
                       '-'
  means not moving at all
*/
char getFacingToward() {
  //1. Read each pin
  max_reading times
 
  int top_left = averageReading(light_top_left_PIN, max_reading);

  int bottom_left = averageReading(light_bottom_left_PIN, max_reading);
  int
  top_right = averageReading(light_top_rigth_PIN, max_reading);
  int bottom_right
  = averageReading(light_bottom_right_PIN, max_reading * 3);
 
  //Show photo
  sensor readings...
  Serial.print(F("Top left-A6: "));
  Serial.println(top_left);

  Serial.print(F("Bottom left-A5: "));
  Serial.println(bottom_left);
  Serial.print(F("Top
  right-A2: "));
  Serial.println(top_right);
  Serial.print(F("Bottom rightA1:
  "));
  Serial.println(bottom_right);
 
  //2. Get max value sides(two
  averaged, see above)
  byte go_direction[4] = {0, 0, 0, 0};
  int toward[4]
  = {0, 0, 0, 0}; //Top, Bottom, Left, Right
  toward[0] = (top_left + top_right)
  / 2 ;
  toward[1] = (bottom_left + bottom_right) / 2 ;
  toward[2] = (top_left
  + bottom_left) / 2;
  toward[3] = (top_right + bottom_right) / 2;
 
  //3.
  Add all side and average,
  //    if each side is within the tolerance then don't
  move Gimbal
  //    average = toward[0] +...toward[3]
  //    average - toward[0]
  .... toward[3], if all within gimbal_movement_tolerance, then no move
  int total_toward
  = 0;
  total_toward += toward[0];
  total_toward += toward[1];
  total_toward
  += toward[2];
  total_toward += toward[3];
  //get average
  int total_average
  = total_toward / 4; //4 sides
  Serial.print(F("total_average:"));
  Serial.println(total_average);

  //    if each side is within the tolerance then don't move Gimbal
  boolean
  shouldMove = false;
  //else move the gimbal
  if (total_average - toward[0]
  > gimbal_movement_tolerance) shouldMove = true;
  if (total_average - toward[1]
  > gimbal_movement_tolerance) shouldMove = true;
  if (total_average - toward[2]
  > gimbal_movement_tolerance) shouldMove = true;
  if (total_average - toward[3]
  > gimbal_movement_tolerance) shouldMove = true;
 
  Serial.print(F("toward
  0 TOP : "));
  Serial.println(toward[0]);
  Serial.print(F("toward 1 BOTTOM:
  "));
  Serial.println(toward[1]);
  Serial.print(F("toward 2 LEFT: "));

  Serial.println(toward[2]);
  Serial.print(F("toward 3 RIGHT: "));
  Serial.println(toward[3]);

  
  //Find the biggest number to decide which side to go,
  // but if four
  values are quite similar, send '-' back to indicate not moving
  char facing
  = ' ' ;
  if (shouldMove) {
    int max_ = 0;
    if (toward[0] > max_)
  {
      max_ = toward[0];
      facing = 'T';
    }
    if (toward[1]
  > max_) {
      max_ = toward[1];
      facing = 'B';
    }
    if (toward[2]
  > max_) {
      max_ = toward[2];
      facing = 'L';
    }
    if (toward[3]
  > max_) {
      max_ = toward[3];
      facing = 'R';
    }
  } else
  {
    facing = '-'; //no need to move
  }
  //  Serial.print(F("shouldMove:
  "));
  //  Serial.println(shouldMove);
  return facing;
}

Downloadable files

Schematic file

This is made with Fritzing

Schematic file

Documentation

3D SG90 Servo 2-axis Servo

This is the 3D file used in this example

https://www.thingiverse.com/thing:2892903?fbclid=IwAR3V7gCUuwR16AMj0QbHZZu0_hXa5w2EaHyFVb7AAzlzgd4ViHfj5boNqRs

3D SG90 Servo 2-axis Servo

This is the 3D file used in this example

https://www.thingiverse.com/thing:2892903?fbclid=IwAR3V7gCUuwR16AMj0QbHZZu0_hXa5w2EaHyFVb7AAzlzgd4ViHfj5boNqRs

Comments

Only logged in users can leave comments

stonez56

0 Followers

•

0 Projects

Intro

Trademarks & CopyrightsWhistleblowingDigital Services ActTerms of ServicePrivacy PolicySecurityCookie Settings

Report content