look this over (compiles but un-tested)
attempts to
- capture sensor peak and angle of peak
- slow motor on side of peak to turn toward peak
- stop motors if peak sensor < some minimum sensor levels
test code is enabled to check pin assignments to move forward and INA and INB Forward/Reverse settings for left & right wheel
#include <Servo.h>
Servo servo_A5;
const byte PinPhotoResistor = A3;
const byte PinInALeft = 4;
const byte PinInBLeft = 5;
const byte PinEnLeft = 10;
const byte PinInARight = 6;
const byte PinInBRight = 7;
const byte PinEnRight = 11;
int ang = 0;
int dir = 1;
const int MinSensorVal = 50;
int sensorValLst;
int sensorValLst2;
int sensorVal;
int sensorValPeak;
int sensorPeakAng;
char s [90];
// -----------------------------------------------------------------------------
void
swivelSensor ()
{
// advance ang and reverse dir at endpoints
ang += dir;
if (ang <= 0 && 180 <= ang)
dir = -dir;
// move sensor servo
servo_A5.write (ang);
// read sensor
sensorVal = analogRead (PinPhotoResistor);
// capture peak
if (sensorValLst2 <= sensorValLst && sensorValLst > sensorVal) {
sensorValPeak = sensorVal;
sensorPeakAng = ang;
}
// update previous values (no quite right at endpoints)
sensorValLst2 = sensorValLst;
sensorValLst = sensorVal;
}
// -----------------------------------------------------------------------------
enum { For, Rev };
void setMotorPwm (
int pwm,
int dir,
byte pinEn,
byte pinInA,
byte pinInB )
{
sprintf (s, " setMotorPwm: en-pin %d, pwm %3d", pinEn, pwm);
Serial.println (s);
if (For == dir) {
digitalWrite (pinInA, HIGH);
digitalWrite (pinInB, LOW);
}
else {
digitalWrite (pinInA, LOW);
digitalWrite (pinInB, HIGH);
}
analogWrite (pinEn, pwm);
}
// -----------------------------------------------------------------------------
const int PWM = 100; // default pwm value
void motorAdjust ()
{
sprintf (s, "moveAdjust: sensor %6d, ang %3d",
sensorValPeak, sensorPeakAng);
Serial.println (s);
int pwmLeft = PWM;
int pwmRight = PWM;
if (MinSensorVal > sensorValPeak) // stop
pwmLeft = pwmRight = 0;
else if (sensorPeakAng < 85) // to left
pwmLeft /= 2; // slow left motor
else if (95 < sensorPeakAng) // to right
pwmRight /= 2; // slow right motor
setMotorPwm (pwmLeft, For, PinEnLeft, PinInALeft, PinInBLeft);
setMotorPwm (pwmRight, For, PinEnRight, PinInARight, PinInBRight);
}
// -----------------------------------------------------------------------------
void loop ()
{
// test code, turn each motor on/off for 2 sec
if (true) {
setMotorPwm (PWM, For, PinEnLeft, PinInALeft, PinInBLeft);
delay (2000);
setMotorPwm ( 0, For, PinEnLeft, PinInALeft, PinInBLeft);
setMotorPwm (PWM, For, PinEnRight, PinInARight, PinInBRight);
delay (2000);
setMotorPwm ( 0, For, PinEnRight, PinInARight, PinInBRight);
}
else {
swivelSensor ();
motorAdjust ();
delay (100);
}
}
// -----------------------------------------------------------------------------
void setup ()
{
Serial.begin (9600);
servo_A5.attach (A5, 500, 2500);
pinMode (PinInARight, OUTPUT);
pinMode (PinInARight, OUTPUT);
pinMode (PinEnRight, OUTPUT);
pinMode (PinInALeft, OUTPUT);
pinMode (PinInALeft, OUTPUT);
pinMode (PinEnLeft, OUTPUT);
setMotorPwm (0, For, PinEnLeft, PinInALeft, PinInBLeft);
setMotorPwm (0, For, PinEnRight, PinInARight, PinInBRight);
}