Проблема ми е следният създадох,робот за ориентиране в лабиринт но използвах AFМotor вместо Sparkfun motor shield.
Тъй като програмирането не ми е най силната страна въпроса ми е некой ще можели да преправи кода за да работи с AFMotor Shield-a.
Ако някой не знае за кой Shield става на въпрос това е :
ТУКТова е страницата от която е взет кода и проекта-
ТУКА кода който трябва да се преправи е това:
#include <SoftPWMServo.h>
//MOTOR SETUP
const int pwm_a = 3; //PWM control for motor outputs 1 and 2 is on digital pin 3
const int pwm_b = 11; //PWM control for motor outputs 3 and 4 is on digital pin 11
const int dir_a = 12; //direction control for motor outputs 1 and 2 is on digital pin 12
const int dir_b = 13; //direction control for motor outputs 3 and 4 is on digital pin 13
const char motor_speed = 200; //set the motor speed (between 0 and 255)
//IR SENSOR SETUP
const int rSensor_pin = 0; //right sensor will be read on ANO
const int fSensor_pin = 5; //front sensor will be read on AN5
//Let's set up some variable that we'll use in the code
int rSensor_val = 0; //this will be the right sensor value ADC reading (between 0 and 1023)
int fSensor_val = 0; //this will be the front sensor value ADC reading (between 0 and 1023)
boolean sweetspot = false; //boolean value initialized to false. When its true, it means the wall on the right of the robot is the "sweet spot"
boolean rSensorHit = false; //boolean value initialized to false. When its true, it means the wall on the robot's right is too close
boolean fSensorHit = false; //boolean value initialized to false. When its true, it means the wall in front of the robot is too close
int sensor_state = 0;
void setup()
{
pinMode(dir_a, OUTPUT); //Set the dir_a pin as an output
pinMode(dir_b, OUTPUT); //Set dir_b pin as an output
digitalWrite(dir_a, HIGH); //Reverse motor direction, 1 high, 2 low
digitalWrite(dir_b, HIGH); //Reverse motor direction, 3 low, 4 high
}
void loop()
{
//This is how I usually do things but you may wish to change it up a bit
//First get all you inputs
read_sensors();
//Then make some decisions based on these inputs
parse_inputs();
//Then do all your outputs at the same time. In this case, determine motor speeds
run_motors();
//I always have a loop timing function
loop_timing();
}
//this function reads the IR value on the analog input pins
void read_sensors()
{
rSensor_val = analogRead(rSensor_pin);
fSensor_val = analogRead(fSensor_pin);
}
//this function then makes decisions based on those inputs
void parse_inputs()
{
if (fSensor_val > 850) fSensorHit = true; //if the front sensor sees a wall set the fSensorHit variable TRUE
//Otherwise, check if the robot is in the "sweet spot"
else if ((rSensor_val > 750)&&(rSensor_val <850)&&(fSensor_val < 850)) sweetspot = true; //If not, then is the wall on the right side of the robot too close? //If so, set the rSensorHit variable TRUE else if (rSensor_val > 850) rSensorHit = true;
//Otherwise make sure all of the sensor flags are set to FALSE
else{
fSensorHit = false;
rSensorHit = false;
sweetspot = false;
}
//reinitialize your sensor variable to a known state
rSensor_val = 0;
fSensor_val = 0;
}
//This routine runs the two motors based on the flags in the previous funciton
void run_motors()
{
//If the wall on the right is too close, turn left away from the wall
if (rSensorHit)left();
//Otherwise, if the wall is in the "sweet spot", then try and stay parallel with the wall
//keeping both wheel speeds the same
else if (sweetspot)forward();
//If the front sensor sees a wall, turn left
else if (fSensorHit)left();
//If neither sensor sees a wall, then move in a really big arc to the right.
//Hopefully, the bot wil be able to pick up a wall
else right();
//Always set your flags to a know value. In this case, FALSE
fSensorHit = false;
rSensorHit = false;
sweetspot = false;
}
void loop_timing()
{
delay(50);
}
//Motor routines called by the run_motors()
void forward() //full speed forward
{
//both wheels spin at the same speed
SoftPWMServoPWMWrite(pwm_a, motor_speed);
SoftPWMServoPWMWrite(pwm_b, motor_speed);
}
void stopped() //stop
{
//turn off both motors
SoftPWMServoPWMWrite(pwm_a, 0);
SoftPWMServoPWMWrite(pwm_b, 0);
}
void left() //stop motor A
{
//turn of the left motor so that the robot vears to the left
SoftPWMServoPWMWrite(pwm_a, motor_speed);
SoftPWMServoPWMWrite(pwm_b, 0);
}
void right() //stop motor B
{
//Here we're running the right motor at a slower speed than the left motor
//Therefore, the bot will travel in a large circle to the right
SoftPWMServoPWMWrite(pwm_a, 50);
SoftPWMServoPWMWrite(pwm_b, motor_speed+50);
}
