Wednesday, May 21, 2014

Glory 3.0 (Code)

Here I left the code for the robot Glory 3.0. There are two codes, one for the robot and the other for the control base. Both codes were made using arduino IDE and using arduino tabs. You have to  instal two libraries for compiling the code, virtualwire and servotimer2. Here I left a zip with the code and the libraries.

Download here


Base Arduino Nano


Arduino_Nano_control

/*******************************************************
 *                Glory 3.0 Code                       *
 * This is the transmitter code.                       *
 *                                                     *
 * Author: Rodrigo Mompó Redoli                        *
 *         with Arduino comunity help                  *
 * Webpage: controlrobotics.rodrigomompo.com           *
 * Email: controlrobotics@rodrigomompo.com             *
 *******************************************************/
#include <LiquidCrystal.h> //Librarys
#include <VirtualWire.h> 

// Variables 
const int Sensor1Pin = A1; // Joystick pins
const int Sensor2Pin = A2;
const int Sensor3Pin = A3;
const int Sensor4Pin = A4; 
const int ledPin = 13;
int selectstatus= 0; 
int Menupag = 1;
int ModoCabeza = 0;
int Laserstatus = 0;
int reading = 0;
int Sensor1Data;
int Sensor2Data;
int Sensor3Data;
int Sensor4Data;
int fire;
long previousMillis;
long currentMillis;

char Sensor1CharMsg[30]; // String were all the varaibles will be stored
LiquidCrystal lcd(8,3,4,5,6,7); // define the pins for the lcd
void setup() {

 // LED 
 pinMode(ledPin,OUTPUT);
 
 // Sensor(s)
 pinMode(Sensor1Pin,INPUT);
 pinMode(Sensor2Pin,INPUT);
 pinMode(Sensor3Pin,INPUT);
 pinMode(Sensor4Pin,INPUT);
 
 pinMode(2,INPUT); // Fire botton 

 lcd.begin(16,2);   // Lcd
 lcd.setCursor(3,0);
 lcd.print("Glory 3.0");
  
 // for debuggin
  Serial.begin(9600); 
 
  // VirtualWire setup
  vw_setup(2000);  // Bits per sec

  delay(1000);
}

void loop() {

  // Read and store Sensor 1 data
  Sensor1Data = analogRead(Sensor1Pin);
  Sensor2Data = analogRead(Sensor2Pin);
  Sensor3Data = analogRead(Sensor3Pin);
  Sensor4Data = analogRead(Sensor4Pin);
  
 //This code is made  to prevent fire several bullets at the same time
currentMillis = millis();
  if(digitalRead(2)== 1 && currentMillis - previousMillis > 1000) {
 
    previousMillis = currentMillis;
  fire = 1;}
  else{fire = 0;}
  //Convert the variables into a string
  sprintf(Sensor1CharMsg, "%d,%d,%d,%d,%d,%d,%d,", Sensor1Data, Sensor2Data, Sensor3Data, Sensor4Data, ModoCabeza, Laserstatus, fire);

  
  //* DEBUG
  Serial.print("Sensor1 Integer: ");
  Serial.print("X");
  Serial.print(Sensor1Data);
  Serial.print("Y");
  Serial.print(Sensor2Data);
  Serial.print("X1");
  Serial.print(Sensor3Data);
  Serial.print("Y1");
  Serial.print(Sensor4Data);
  Serial.print("Sensor1Datamsg");
  Serial.print(Sensor1CharMsg);
  Serial.println("//");

  // END DEBUG*/
 
digitalWrite(13,LOW); // Turn on a light to show transmitting
 vw_send((uint8_t *)Sensor1CharMsg, strlen(Sensor1CharMsg));
 vw_wait_tx(); // Wait until the whole message is gone
  // Turn off a light after transmission
 delay(2);
digitalWrite(13,HIGH); // Turn off a light to show transmitting

reading = analogRead(0); // read the botons 
Serial.println(analogRead(0));// For debug
DisplayData();

} 
DisplayData
void DisplayData(){
// this code lets you select differents menus
  if(reading == 0){
    if(Menupag==3){}
    else{Menupag++;}
  }
  if(reading == 481){ 
    
    if(Menupag==1){}
    else{Menupag--;}
  }
 switch(Menupag){
   case 1:
   Menu1();
   break;
   case 2:
   Menu2();
   break;
   case 3:
   Menu3();
   break;

   
 }
}
Menu1
void Menu1(){
 // This code  display informatión about the movement of the robot 
  int variable =map(Sensor1Data,0,1023,0,3);
  lcd.clear();
  lcd.setCursor(0,0);
  lcd.print("Direcion:");
  lcd.setCursor(10,0);
  switch( variable ){
   case 0:
   lcd.print("Atras");
   
   break;
   case 1:
   switch(map(Sensor2Data,0,1023,0,3)){
   case 0:
   lcd.print(" <== ");
   break;
   case 1:
   lcd.print("Stop");
   break;
   case 2:
   lcd.print(" ==> ");
   break;
   }
   
   break;
   case 2:
   lcd.print("Ade");
   break;
   case 3:
   lcd.print("Ade");
   break;
  }
  lcd.setCursor(0,1);
  lcd.print("X: ");
  lcd.print(Sensor1Data);
  lcd.print("  Y: ");
  lcd.print(Sensor2Data);
}
  
Menu2
void Menu2(){
 // this menu display information of the head of the robot
  lcd.clear();
  lcd.setCursor(0,0);
  lcd.print("ModoCabeza: ");
  lcd.setCursor(11,0);
//This part of the code let's you to select diferent modes for the head
  if(reading == 722){
    switch(ModoCabeza){
     case 0:
     ModoCabeza = 1;
     break;
     case 1:
     ModoCabeza = 2;
     break;
     case 2:
     ModoCabeza = 0;
     break;
    }
  }

  switch(ModoCabeza){
    case 0:
    lcd.print("Movil");
    break;
    case 1:
    lcd.print(" Fijo");
    break;
    case 2:
    lcd.print("Ultra");
    break;
  }
  
  lcd.setCursor(0,1);
  lcd.print("X: ");
  lcd.print(Sensor4Data);
  lcd.print("  Y: ");
  lcd.print(Sensor3Data);
}  
Menu3
void Menu3(){
 // display the laser status and let you chage it
  lcd.clear();
  lcd.setCursor(0,0);
  lcd.print("Laser: ");
  lcd.setCursor(8,0);
  if(reading == 722){
    switch(Laserstatus){
     case 0:
     Laserstatus= 1;
     break;
     case 1:
     Laserstatus = 0;
     break;
    }
  }
  switch(Laserstatus){
    case 0:
    lcd.print("OFF");
    break;
    case 1:
    lcd.print("ON");
    break;
  }
 
}

Robot Glory 3.0 Arduino Mega


Arduino_Mega_en_robot
/*******************************************************
 *                Glory 3.0 Code                       *
 * This is the code that Glory runs. You need also     *
 * a robot controler with the transmitter code.        *
 *                                                     *
 * Author: Rodrigo Mompó Redoli                        *
 *         with Arduino comunity help                  *
 * Webpage: controlrobotics.rodrigomompo.com           *
 * Email: controlrobotics@rodrigomompo.com             *
 *******************************************************/
 
#include <VirtualWire.h>//Librarys
#include <ServoTimer2.h>// You can`t use VirtualWire and Servo library together,
// both use the same Arduino Timer. You nedd to use ServoTimer2.h that use Timer2 of the arduino.

// This code have extracts from differents tutorials on the web that I used long time ago, so I can remmeber the authors 
#define MOTOR1_DIR_PIN  4  // Dir 
#define MOTOR1_PWM   5 // CH2 Left Motor  
  
#define MOTOR2_DIR_PIN  7  // Dir 
#define MOTOR2_PWM   6 // CH3 Right Motor
  
#define MOTOR_DIR_FORWARD  0  
#define MOTOR_DIR_BACKWARD   1
//Variables for Pan/Tilt servomotors
int oneb; int previousoneb; int servo1x;
int twob; int previoustwob; int servo2x;
//Creating servo objects
ServoTimer2 one;
ServoTimer2 two;

// Values from RF
int Sensor1Data;
int Sensor2Data;
int Servo1;
int Servo2;
int ModoCabeza;
int Laser;
int finish;
//Pin for the laser
int Laserpin = 2;
int Canonpin = 3;
int sensorcanon = 8;
int firesensor;
long previousMillis;
long currentMillis;
// RF Transmission container
char Sensor1CharMsg[26]; 


void setup() {
  digitalWrite(Canonpin, HIGH);
  Serial.begin(9600);//For debuggin
  // Setup pins for motor 2  
   pinMode(MOTOR1_DIR_PIN,OUTPUT);   
   pinMode(MOTOR1_PWM,OUTPUT);  
     
  // Setup pins for motor 2  
   pinMode(MOTOR2_DIR_PIN,OUTPUT);  
   pinMode(MOTOR2_PWM,OUTPUT);
  // Sets the digital pins as output
  pinMode(13, OUTPUT);
  pinMode(Laserpin, OUTPUT);
  pinMode(Canonpin, OUTPUT);
  pinMode(sensorcanon, INPUT);

//Attach the servo to enable their use  
one.attach(9);
two.attach(10);
    
    // VirtualWire
    //(see controlrobotics.rodrigomompo.com for more information and tutorials) 
    vw_setup(2000);//Bits per second
    vw_set_rx_pin(11);//Set receiver Pin  
    // Start the receiver PLL running
    vw_rx_start(); 
Serial.print("Ready");//For debuggin

} // END void setup

void loop(){
 
TakingData();//Call a funtion that takes the data from RF 
    
//Control del laser 
if(Laser==1){digitalWrite(Laserpin,LOW);}
else{digitalWrite(Laserpin,HIGH);}

Serial.println(Laser);
MotorControl();//Control the robot motors
// For controling the cannon
currentMillis = millis();
  if(firesensor == 1 && currentMillis - previousMillis > 500) {
    
    previousMillis = currentMillis;
digitalWrite(Canonpin,LOW);
delay(10);
  fire();}

digitalWrite(Canonpin,HIGH);

Serial.print(digitalRead(sensorcanon));
PanTilt();//Loop for controling the Pan/Tilt Head
 
}
Cannon
// This loop runs until the sensor of the cannon it's activeted when a bullet has been shooted
void fire(){
while (digitalRead(sensorcanon) == 0){
digitalWrite(Canonpin,LOW);
 }
}
MotorControl
// This funtion Control the motors
// for moving the robot
void MotorControl(){
int speed1 = map(Sensor1Data,500,69,0,255); 
int speed2 = map(Sensor1Data,500,1023,0,255); 
int variable =map(Sensor2Data,0,1023,0,3);//Take Y coordinate
switch( variable ){
   case 0:
   // The robot go Backward 
   motorStart(1, MOTOR_DIR_BACKWARD);    
   setSpeed(1, 
    100);  
   motorStart(2, MOTOR_DIR_BACKWARD);        
   setSpeed(2, 100);  
   delay(2); 
   
   break;
   case 1:
   // In this case three new posibilities apeared
     switch(map(Sensor1Data,0,1023,0,3)){
     case 0:
      //Robot goes right
        motorStart(2, MOTOR_DIR_BACKWARD);     
          setSpeed(1, speed1);  
        motorStart(1, MOTOR_DIR_FORWARD);        
          setSpeed(2, speed1);
       delay(2); 
       break;
     case 1:
     //Robot stop
       motorStop(1);
       motorStop(2);
       break;
     case 2:
     //Robot goes left
      motorStart(2, MOTOR_DIR_FORWARD);    
         setSpeed(1,speed2);  
      motorStart(1, MOTOR_DIR_BACKWARD);        
         setSpeed(2,speed2 );  
       delay(2);
        break;
   }
   
   break;
   case 2:
  
  // The motor go Fordward
  motorStart(1, MOTOR_DIR_FORWARD);    
  setSpeed(1,100);  
  motorStart(2, MOTOR_DIR_FORWARD);        
  setSpeed(2, 100);  
  delay(2); 
   break;
  }
}
Motor_Control_Funtions
/*Here there are three funtions:
  setSpeed();
  motorStart();
  motorStop();
  
 All funtions are used for motor control 
*/


void setSpeed(char motor_num, char motor_speed)  
{  
   if (motor_num == 1)  
   {  
      analogWrite(MOTOR1_PWM, motor_speed);  
   }     
   else  
   {  
      analogWrite(MOTOR2_PWM, motor_speed);  
   }  
}  
  
void motorStart(char motor_num, byte direction)  
{  
    
   char pin_dir;  
    
     
   if (motor_num == 1)  
   {  
      pin_dir = MOTOR1_DIR_PIN ;  
       
   }     
   else  
   {  
      pin_dir = MOTOR2_DIR_PIN ;  
         
   }  
    
   switch (direction)  
   {  
     case MOTOR_DIR_FORWARD:  
     {  
       digitalWrite(pin_dir,LOW);  
                   
     }  
     break;   
            
     case MOTOR_DIR_BACKWARD:  
     {  
        digitalWrite(pin_dir,HIGH);  
                  
     }  
     break;           
   }  
}  
  
void motorStop(char motor_num)  
{  
   setSpeed(motor_num, 0);  
   if (motor_num == 1)  
   {  
     digitalWrite(MOTOR1_DIR_PIN ,HIGH);  
       
   }  
   else  
   {  
     digitalWrite(MOTOR2_DIR_PIN ,HIGH);  
          
   }  
}  
PanTilt
/*
This funtion control the head with 
the camera and the shootcanon*/

void PanTilt(){


 if(ModoCabeza== 0){  
 int onea = map(Servo1,950,0,2175,600);
 one.write(onea + 50);//vertical
 int twoa = map(Servo2,940,30,600,2000);
 two.write(twoa + 100);// horizontal
 previousoneb = onea;
 previoustwob = twoa;
 }

 if (ModoCabeza == 1){
   
   if(Servo1<350&&previousoneb<2300){oneb = previousoneb+10; one.write(oneb); previousoneb = oneb;}
   if(Servo1>550&& previousoneb>754){oneb = previousoneb-10; one.write(oneb); previousoneb = oneb;}
   if(Servo2<550&&previoustwob<2300) {twob = previoustwob+10; two.write(twob);previoustwob = twob;}
   if(Servo2>400&& previoustwob>754){twob = previoustwob-10; two.write(twob);previoustwob = twob;}
  
   delay(2);
   
 } 
 if (ModoCabeza == 2){
   
   if(Servo1<350&&previousoneb<2300){oneb = previousoneb+3; one.write(oneb); previousoneb = oneb;}
   if(Servo1>550&& previousoneb>754){oneb = previousoneb-3; one.write(oneb); previousoneb = oneb;}
   if(Servo2<550&&previoustwob<2300){twob = previoustwob+3; two.write(twob);previoustwob = twob;}
   if(Servo2>400&& previoustwob>754){twob = previoustwob-3; two.write(twob);previoustwob = twob;}
  

}
delay(10);

}

TakingData
/*This funtion take the data from the RF
and store it in diferent integres for controling 
the robot*/

void TakingData(){ 
 
  //For taking data from RF and store it 
     uint8_t buf[VW_MAX_MESSAGE_LEN];
     uint8_t buflen = VW_MAX_MESSAGE_LEN;
     
 //Taking the data from the control base
     if (vw_get_message(buf, &buflen)) 
     {
  int i; 
         digitalWrite(13,HIGH);
   
         // Message with a good checksum received, dump it. 
          for (i = 0; i < buflen; i++){            
           // Fill Sensor1CharMsg Char array with corresponding 
           // chars from buffer.   
           Sensor1CharMsg[i] = char(buf[i]);}
         
  //A funtion used for taking a string with all the data and convert it in differents number variables 
       sscanf(Sensor1CharMsg, "%d,%d,%d,%d,%d,%d,%d",&Sensor1Data, &Sensor2Data,&Servo1,&Servo2,&ModoCabeza,&Laser,&firesensor); // Convierte un string en un array
       Serial.print("Read"); //For debuggin
       Serial.print(Sensor1CharMsg); //For debuggin

       
    }

 delay(10);
 digitalWrite(13,LOW);  
}

2 comments:

  1. Hi! Rodrigo,

    I am new to Arduino and love your project.

    I have been studying your Glory 3.0 Code and have a question. I am familiar with calling function in other languages but have never worked with C++ which I believe Arduino uses. When I compile the MEGA code I get an error "was not declared in this scope" for the Cannon, Motor_Control_Function, MotorControl, PanTilt and TalkingData functions. I assume it has something to do with calling them. How is this done?

    Thanks
    Ralph

    ReplyDelete
  2. Hello

    Thanks you very much for your interst, sorry for make you wait so long for an answer. In arduino you can make new tabs in order to make your code more clear. In my case I created functions in those tabs to make the code more legible.

    For use the code you need to introduce in the main code folder (Arduino_Mega_en_Robot) the ino files corresponding to all the function that I made in different tabs.

    And othe way is to put all this functions in the same file at the end.

    I hope this solve your problem.

    ReplyDelete