Alat yang dibutuhkan :
Sensor api 1 x 5ch
2x fan kipas modul untuk robot
2 x mini motor dc Gearbox + roda
Shield Driver l293d
Rangkaian skematik
pemograman :
AF_DCMotor MotorL(1); // Motor for drive Left on M1
AF_DCMotor MotorR(2); // Motor for drive Right on M2
AF_DCMotor fanMotor(3); // Motor for Fan on M3
int val;
void setup() {
Serial.begin(115200); // set up Serial library at 115200 bps
Serial.println("*SMARS Firefighter Mod*");
// Set the speed to start, from 0 (off) to 255 (max speed)
MotorL.setSpeed(255);
MotorL.run(FORWARD);
MotorL.run(RELEASE);
MotorR.setSpeed(255);
MotorR.run(FORWARD);
MotorR.run(RELEASE);
fanMotor.setSpeed(255);
fanMotor.run(FORWARD);
fanMotor.run(RELEASE);
}
void loop() {
flameangle(); // aligns the flame on the central sensor
setdistance(); // move the robot in the right position
int num= maxflame('v');
if (num> 996) { // verify the flame intensitiy
fanMotor.run(FORWARD); // actives the fan motor for 2 seconds
delay(2000);
fanMotor.run(RELEASE); // deative the fan motor
};
}
//---------------------------------------------------------------------------
// The function maxflame() gets the values from the 5 channels and returns
// the position where it finds the max value (if you give 'p' as argument l)
// or the max value (if you give 'v' as argument l)
// connections:
//----------------------
// sensor arduino-
// - A1 -. A0 -
// - A2 -. A1 -
// - A3 -. A2 -
// - A4 -. A3 -
// - A5 -. A4 -
//----------------------
//
int maxflame(char l) {
int data[5]; // creating an array where store the sensor values
int i;
for (i=0; i<5; i++) { // read the sensor values and store them into the array
data[i]= analogRead(i);
};
int mv=0; // max value
int mn; // max value position
for (i=0; i<5; i++) { // check all the array's elements and sets mv to the maximum value measured and mn to the array's position i
if (data[i]> mv) {
mv=data[i];
mn=i;
}
};
if(l=='p') { // returns the position of the maximum value
return mn;
};
if(l=='v') { // returns the maximum value
return mv;
}
}
//---------------------------------------------------------------------------------------
// The function flameangle() move aligns the robot with the flame
void flameangle(){
val= maxflame('p'); //store the position of the maxflame into the variable val
if(val<2) { // if the position is on the right it makes the robot turn counterclockwise until the max val is measured on the central "eye"
while (val!=2) {
MotorL.setSpeed(80);
MotorR.setSpeed(80);
MotorL.run(FORWARD);
MotorR.run(BACKWARD);
val= maxflame('p');
};
};
Serial.println(val);
if(val>2) { // if the position is on the link it makes the robot turn clockwise until the max val is measured on the central "eye"
while(val!=2) {
MotorL.setSpeed(80);
MotorR.setSpeed(80);
MotorR.run(FORWARD);
MotorL.run(BACKWARD);
val= maxflame('p');
};
};
if (val==2) { // When the robot is in front of the flame, it deactivates the motors
MotorL.run(RELEASE);
MotorR.run(RELEASE);
}
}
//---------------------------------------------------------------------------------------
// The function setdistance() makes the robot move forward or backward until it is at a choosed distance
void setdistance() {
int md=998; //minimun distance
int maxd=980; //maximum distance
int tub=996; //tollerance
val= maxflame('v'); //if the robot it's too near the flame, it go backward
if(val> md) {
MotorL.run(BACKWARD);
MotorR.run(BACKWARD);
}
if(val>maxd && val<tub) { //if the robot it's too far from thr flame, it go forward
MotorL.run(FORWARD);
MotorR.run(FORWARD);
}
if(val>tub&& val<md) { // if it's at the right distance, it stops.
MotorL.run(RELEASE);
MotorR.run(RELEASE);
}
}
0 komentar:
Post a Comment