Unlimited CNC Robot Blog #6
I wanted to post this blog before I post my final blog tonight. I found a big error in my code and wanted to update it now.
The error I made was I had my line sensors reversed to what they needed to be for the right reading. I had a "<" instead of ">" and reverse of that.
I added new code as well, thanks to DAB for the suggestion!! Excellent idea and it works great!!
Below is the corrected code as well as the newly added code:
/**Unlimited CNC Robot**/
#include <IRremote.h> //including infrared remote header file - This is used to start the robot and emergency stop
#include <pitches.h> //include sound
int RECV_PIN = 7; // the pin where you connect the output pin of IR sensor
IRrecv irrecv(RECV_PIN);
decode_results results;
/**set control port - DFRobot L298P**/
const int E1Pin = 10;
const int M1Pin = 12;
const int E2Pin = 11;
const int M2Pin = 13;
/**inner definition**/
typedef struct {
byte enPin;
byte directionPin;
} MotorContrl;
const int M1 = 0;
const int M2 = 1;
const int MotorNum = 2;
const MotorContrl MotorPin[] = { {E1Pin, M1Pin}, {E2Pin, M2Pin} } ;
const int Forward = LOW;
const int Backward = HIGH;
//Sensor Connection
const int left_sensor_pin =A0;
const int right_sensor_pin =A1;
const int laser_on_sensor_pin =A2; /**Right**/
const int laser_off_sensor_pin =A3; /**Left**/
int left_sensor_state;
int right_sensor_state;
int laser_on_sensor_state;
int laser_off_sensor_state;
int turn_delay = 10;
int laser_status = 0; /**0 = Laser status is off, 1 = Laser status is On**/
int ircontrol = 0;
int speakernote = NOTE_F4;
int speakernote2 = NOTE_G4;
int speakernote3 = NOTE_A5;
/**program**/
void setup() {
Serial.begin(9600);
irrecv.enableIRIn();
ircontrol = 0;
initMotor();
pinMode(9, OUTPUT); /**Power to Laser & Fan**/
pinMode(4, OUTPUT); //LED Green
pinMode(5, OUTPUT); //LED Yellow
pinMode(6, OUTPUT); //LED Red
pinMode(8, OUTPUT); //LED Speakernote
}
void loop() {
digitalWrite(5, LOW); //LED Yellow - Power off
digitalWrite(6, LOW); //LED Green - Power off
Serial.println("Waiting for IR Signal to start");
digitalWrite(4, HIGH); //LED Green - Power On
if(irrecv.decode()){ // IR receive results
ircontrol = 1;
delay(500);
digitalWrite(5, HIGH); //LED Yellow - 1st warning
delay(1000);
tone(8, speakernote, 600); //Sound warning 1
delay(1000);
noTone(8);
tone(8, speakernote, 600); //Sound warning 2
delay(1000);
noTone(8);
tone(8, speakernote, 600); //Sound warning 3
delay(1000);
noTone(8);
tone(8, speakernote, 600); //Sound warning 4
delay(1000);
noTone(8);
tone(8, speakernote3, 600); //Sound warning 5
delay(1000);
noTone(8);
digitalWrite(6, HIGH); //LED Red - Robot Running
delay(2000);
}
if(ircontrol != 0){
do{
Serial.println("Robot in Running Mode");
int value;
irrecv.resume();
left_sensor_state = analogRead(left_sensor_pin);
right_sensor_state = analogRead(right_sensor_pin);
laser_on_sensor_state = analogRead(left_sensor_pin);
laser_off_sensor_state = analogRead(right_sensor_pin);
if(laser_status = 0)
{
if(laser_on_sensor_state < 500)
{
Serial.println("Laser On/Fan On");
setMotorSpeed( M1, 0 );
setMotorSpeed( M2, 0 );
digitalWrite(9, HIGH); //Laser & Fan on
tone(8, speakernote2, 600); //Laser On signal
delay(250);
tone(8, speakernote3, 600); //Laser On signal
delay(250);
noTone(8);
laser_status = 1; /**Laser status is now on**/
}
}
if(laser_status = 1)
{
if(laser_off_sensor_state < 500)
{
Serial.println("Laser Off/Fan Off");
setMotorSpeed( M1, 0 );
setMotorSpeed( M2, 0 );
digitalWrite(9, LOW); //Laser & Fan off
delay(200);
tone(8, speakernote3, 600); //Laser Off signal
delay(250);
tone(8, speakernote2, 600); //Laser Off signal
delay(250);
noTone(8);
laser_status = 0; //Laser status is now off
}
}
if(right_sensor_state > 500 && left_sensor_state < 500)
{
Serial.println("turning right");
setMotorDirection( M2, Forward );
setMotorSpeed( M2, 100 );
setMotorSpeed( M1, 0 );
delay(turn_delay);
}
if(right_sensor_state < 500 && left_sensor_state > 500)
{
Serial.println("turning left");
setMotorDirection( M1, Forward );
setMotorSpeed( M1, 100 );
setMotorSpeed( M2, 0 );
delay(turn_delay);
}
if(right_sensor_state > 500 && left_sensor_state > 500)
{
Serial.println("going forward");
setMotorDirection( M1, Forward );
setMotorSpeed( M1, 100 );
setMotorDirection( M2, Forward );
setMotorSpeed( M2, 100 );
delay(turn_delay);
}
if(right_sensor_state < 500 && left_sensor_state < 500)
{
Serial.println("stop");
setMotorSpeed( M1, 0 );
setMotorSpeed( M2, 0 );
digitalWrite(9, LOW); //Laser & Fan off
laser_status = 0; //Laser status is off
ircontrol = 0;
tone(8, speakernote3, 600); //Laser Off signal
delay(250);
tone(8, speakernote2, 600); //Laser Off signal
delay(2000);
noTone(8);
}
if(irrecv.decode()){ // IR receive results
ircontrol = 0;
Serial.println("Emergency Stop Pressed");
setMotorSpeed( M1, 0 );
setMotorSpeed( M2, 0 );
digitalWrite(9, LOW); //Laser & Fan off
laser_status = 0; //Laser status is off
delay(2000);
tone(8, speakernote3, 600); //Laser Off signal
delay(250);
tone(8, speakernote2, 600); //Laser Off signal
delay(250);
tone(8, speakernote, 600); //Shutdown signal
delay(200);
tone(8, speakernote, 600); //Shutdown signal
delay(200);
tone(8, speakernote, 600); //Shutdown signal
delay(200);
noTone(8);
}
}while(ircontrol != 0);
}
digitalWrite(5, HIGH); //LED Yellow - Off
digitalWrite(6, HIGH); //LED Red - Off
ircontrol = 0;
irrecv.resume();
}
/**functions**/
void initMotor() {
int i;
for ( i = 0; i < MotorNum; i++ ) {
digitalWrite(MotorPin[i].enPin, LOW);
pinMode(MotorPin[i].enPin, OUTPUT);
pinMode(MotorPin[i].directionPin, OUTPUT);
}
}
/** motorNumber: M1, M2
direction: Forward, Backward **/
void setMotorDirection( int motorNumber, int direction ) {
digitalWrite( MotorPin[motorNumber].directionPin, direction);
}
/** speed: 0-100 * */
inline void setMotorSpeed( int motorNumber, int speed ) {
analogWrite(MotorPin[motorNumber].enPin, 255.0 * (speed / 100.0) ); //PWM
}
The final blog coming tonight!!
Dale Winhold