Multifunction Mobile Robot Rev2

LED Blinking, Walking with Cytron Servo, Displaying RFID, Multi-function Mobile Robot......

Multifunction Mobile Robot Rev2

Postby dausvilla77 » Thu Nov 28, 2013 2:07 am

This robot is designed to detect black line color.
http://www.cytron.com.my/viewProduct.ph ... bot%20Rev2

Can anybody help me how can I edit the source code to make the robot can detect white line color instead of black line color.
Here is attached the source code.
CODE: SELECT_ALL_CODE
//============================================================================================================
//   Author            :CYTRON   
//   Project            :PR23
//   Project description   :Simple line follow
//  Version          :v1.4
//============================================================================================================

//   include
//============================================================================================================
#include <pic.h>

//   configuration
//============================================================================================================
__CONFIG ( 0x3F32 );

//   define
//============================================================================================================
#define sw1         RE0
#define sw2         RE1
#define motor_ra   RC0
#define motor_rb   RC3
#define motor_la   RC4
#define motor_lb   RC5

#define s_left      RB0
#define s_mleft      RB1
#define s_mright   RB2
#define s_right      RB3

#define buzzer       RE2

#define   rs         RB7
#define   e         RB6
#define   lcd_data   PORTD
#define b_light     RB5
#define SPEEDL      CCPR1L
#define SPEEDR      CCPR2L

#define   CHANNEL0   0b10000001   // AN0 ( Ultrasonic )
#define   CHANNEL1   0b10001001   // AN1 ( Distance sensor )

//skps protocol
#define   p_select      0
#define p_joyl         1
#define p_joyr         2
#define p_start         3
#define p_up         4
#define p_right         5
#define p_down         6
#define p_left         7
#define   p_l2         8
#define   p_r2         9
#define p_l1         10
#define p_r1         11
#define p_triangle      12
#define p_circle      13
#define p_cross         14
#define   p_square      15
#define p_joy_lx      16
#define   p_joy_ly      17
#define p_joy_rx      18
#define p_joy_ry      19
#define p_joy_lu      20
#define p_joy_ld      21
#define p_joy_ll      22
#define p_joy_lr      23
#define p_joy_ru      24
#define p_joy_rd      25
#define p_joy_rl      26
#define p_joy_rr      27

#define   p_con_status   28
#define p_motor1      29
#define p_motor2      30

#define RX_PIN       RC7
#define TX_PIN       RA2
#define BOT_ADD      100



//   global variable
//============================================================================================================
unsigned char data[6] = {0};
const unsigned char line [] = {"1.LINE FOLLOW"};
const unsigned char US[] = {"2.Ultrasonic"};
const unsigned char AS[] = {"3.Analog Sensor"};
const unsigned char XB[] = {"4.SKXBEE"};
const unsigned char SKPS[] = {"5.SKPS"};
const unsigned char *mode [5] = {&line[0],&US[0],&AS[0],&XB[0],&SKPS[0]};

unsigned int result;
unsigned int To=0,T=0,TH=0;
unsigned char REC;
unsigned char i=0,raw;

unsigned int us_value (unsigned char mode);

//   function prototype
//============================================================================================================
void init(void);
void delay(unsigned long data);
void send_config(unsigned char data);
void send_char(unsigned char data);
void e_pulse(void);
void lcd_goto(unsigned char data);
void lcd_clr(void);
void send_string(const char *s);
void dis_num(unsigned long data);

void line_follow(void);
void ultrasonic(void);
void wireless_xbee(void);
void analog_sen(void);
void SKPS_PScon(void);

void forward(void);
void stop (void);
void backward (void);
void reverse (void);
void left(void);
void right(void);

void uart_send(unsigned char data);
unsigned char uart_rec(void);
unsigned char skps(unsigned char data);
void skps_vibrate(unsigned char motor, unsigned char value);
void read_adc(char config);


//   interrupt prototype
//============================================================================================================
static void interrupt isr(void)
{
   if (TMR0IF)                        // TMR0 is overflow
   {
      TMR0IF = 0;                   // clear flag bit
      To +=0x100;                     // count number of TMR0 overflow ( make it to 16bit TMR)
   }
      
   if(RBIF)                        // there is change bit on RB4-RB7
   {
      RBIF = 0;                     //                                     ____
      if (RB4)                     // Rb4 is 1 mean is rising form 0  __|         
      {
         TMR0 = 0;                  // clear all counter involved, start new count for period of RB4 high
         To = 0;
      }
                                 //                           ___
      else TH = TMR0 + To;            // RB4 is 0 mean is falling form 1       |_____  // save TH, RB4 high period
   }

   if(RCIF)
   {
      RCIF = 0;                     // clear flag bit

      if (RCREG == 'R') data[i=0]= RCREG;            // check if start byte 'R' is met       
      else if (RCREG == 100) data[i=0]= RCREG;      // check if start byte 'd'(decimal 100) is met
      if ((data[0] == 'R'))data [i++] = RCREG;      // save the data in data array
      if (i>4) i = 4;                           // if the data array reached max, set the index to 4
   }
}


//   main function
//============================================================================================================
void main(void)
{

   unsigned char m=0,i =0;
   delay(20000);                           
   init();                                 // initiate cnfiguration and initial condition
   buzzer = 1;                              // inditcate the circuit is on with beep
   lcd_clr();                              // clear the LCD screen
   send_string("Select mode");                  // display "select mode"
   lcd_goto(20);                           // move to 2nd line
   send_string(mode[m]);                     // display string according to the mode
   buzzer = 0;                              // stop beep

   while(1)                              // loop
   {
      if( !sw1)                           // if button SW1 is pressed
      {
         while(!sw1);                     // wait ubtul button is released
         m++;
         if ( m > 4) m = 0;                  // if mode is added more than three, set to zero
         lcd_goto(20);                     // start display at 20
         send_string(mode[m]);               // display string depend on mode
         send_string("       ");               // space to overwrite long words
      }
      
      if (!sw2)                           // if button SW2is pressed
      {
         while(!sw2);                     // wait until button is released
         switch(m)                        // check what is the current mode, execute the mode
         {

            case 0 :   line_follow();         // mode 1 : line follow
                     break;
            case 1 :   ultrasonic();         // mode 2 : ultrasonic mode
                     break;
            case 2 :   analog_sen();         // mode 3 : analog sensor mode
                     break;
            case 3 :   wireless_xbee();      // mode 4 : wireless xbee mode
                     break;
            case 4 :    SKPS_PScon();         // mode 5 : PS2 Controller Mode
                     break;
            default :   ;         
         }
      }
   }
}

//===========================================================================================================
// Initailization
// Description : Initialize the microcontroller
//============================================================================================================
void init()
{
   // ADC configuration   
   ADCON1 = 0b10000100;            //set RA0 and RA1 as Analog Input, left justified

   // setup for capture pwm
   RBIE = 1;                     // enable interrupt on change of port B

   // motor PWM configuration
   PR2 = 255;                  // set period register
   T2CON =     0b00000100;         //
   CCP1CON =   0b00001100;         // config for RC1 to generate PWM   ( for more detail refer datasheet section 'capture/compare/pwm')
   CCP2CON =   0b00001100;         // config for RC2 to generate PWM

   // Tris configuration (input or output)
   TRISA = 0b00000011;            //set RA0 and RA2 pin as input,other as output
   TRISB = 0b00011111;            //set RB0-RB4 pin as input, other as output
   TRISC = 0b10000000;            //set PORTC pin as output
   TRISD = 0b00000000;            //set all PORTD pin as output
   TRISE = 0b00000011;

   // TMR 0 configuation
   T0CS = 0;                  
   PSA = 0;                  
   PS2 = 1;                  // prescale 1:32
   PS1 = 1;                  //
   PS0 = 1;                  //
   TMR0IE = 1;                  // TMR0 Interrupt
   TMR0 = 0;

   //setup UART
   SPBRG = 0x81;                  //set baud rate to 9600 for 20Mhz
   BRGH = 1;                     //baud rate high speed option
   TXEN = 1;                     //enable transmission
   TX9 = 0;
   CREN = 1;                     //enable reception
   SPEN = 1;                     //enable serial port
   RX9 = 0;
   RCIE = 1;                     //enable interrupt on eachdata received

   // enable all unmasked interrupt   
   GIE = 1;
   PEIE = 1;

   // LCD configuration
   send_config(0b00000001);      //clear display at lcd
   send_config(0b00000010);      //Lcd Return to home
   send_config(0b00000110);      //entry mode-cursor increase 1
   send_config(0b00001100);      //diplay on, cursor off and cursor blink off
   send_config(0b00111000);      //function

   TX_PIN = 1;
   b_light = 0;
   buzzer = 0;
   stop();                                 
}

//============================================================================================================
// Mode subroutine
//============================================================================================================
// Mode 1 : line follow subroutine
// Description: Program for the robot to follow line
// For more detail about line follow concept please refer PR5
//============================================================================================================
void line_follow()                     
{
   unsigned char memory;

   lcd_clr();                        // clear lcd screen
   send_string("Position");            // display "position" string


   while(1)
   {
      if ((s_left==1)&&(s_mleft==0)&&(s_mright==0)&&(s_right==0))              // if only sensor left detected black line
      {   forward();                                                // motor forward
         SPEEDL = 0;                                                // left motor speed is 0
         SPEEDR = 255;                                             // right motor speed is 255(full speed)
         memory = PORTB&0b00001111;                                    // save current sensor position
         lcd_goto(20);                                             // lcd go to 2nd line 1st character
         send_string ("right  ");                                    // display "right"mean the robot's position is on the right side of the line
      }
      else if ((s_left==1)&&(s_mleft==1)&&(s_mright==0)&&(s_right==0))         // if only sensor left detected black line
      {   forward();                                                // motor forward
         SPEEDL = 180;                                             // left motor speed is 180
         SPEEDR = 255;                                             // right motor speed is 255(full speed)
         memory = PORTB&0b00001111;
         lcd_goto(20);
         send_string ("m_right2");
      }
      else if ((s_left==0)&&(s_mleft==1)&&(s_mright==0)&&(s_right==0))         // if only sensor middle left detected black line
      {   forward();                                                // motor forward
         SPEEDL = 200;                                             // left motor speed is 200
         SPEEDR = 255;                                             // right motor speed is 255(full speed)
         memory = PORTB&0b00001111;
         lcd_goto(20);
         send_string ("m_right1  ");
      }
      else if ((s_left==1)&&(s_mleft==1)&&(s_mright==1)&&(s_right==0))         // if sensor middle left and sensor left detected black line
      {   forward();                                                // motor forward
         SPEEDL = 200;                                             // left motor speed is 200
         SPEEDR = 255;                                             // right motor speed is 255(full speed)
         memory = PORTB&0b00001111;
         lcd_goto(20);
         send_string ("m_right1  ");
      }
      else if ((s_left==0)&&(s_mleft==1)&&(s_mright==1)&&(s_right==0))         // if sensor middle left and sensor middle right detected black line
      {   forward();                                                // motor forward
         SPEEDL = 255;                                             // left motor speed is 255(full speed)
         SPEEDR = 255;                                             // right motor speed is 255(full speed)
         memory = PORTB&0b00001111;
         lcd_goto(20);
         send_string ("middle ");
      }
      else if ((s_left==0)&&(s_mleft==0)&&(s_mright==1)&&(s_right==0))         // if only sensor middle right detected black line
      {   forward();                                                // motor forward
         SPEEDL = 255;                                             // left motor speed is 255(full speed)
         SPEEDR = 200;                                             // right motor speed is 200
         memory = PORTB&0b00001111;
         lcd_goto(20);
         send_string ("m_left1   ");
      }
      else if ((s_left==0)&&(s_mleft==1)&&(s_mright==1)&&(s_right==1))         // if sensor middle left, sensor middle right and sensor right detected black line
      {   forward();                                                // motor forward
         SPEEDL = 255;                                             // left motor speed is 255(full speed)
         SPEEDR = 200;                                             // right motor speed is 200
         memory = PORTB&0b00001111;
         lcd_goto(20);
         send_string ("m_left1   ");
      }
      else if ((s_left==0)&&(s_mleft==0)&&(s_mright==1)&&(s_right==1))         // if sensor right and sensor middle right detected black line
      {   forward();                                                // motor forward
         SPEEDL = 255;                                             // left motor speed is 255(full speed)
         SPEEDR = 180;                                             // right motor speed is 180
         memory = PORTB&0b00001111;
         lcd_goto(20);
         send_string ("m_left2 ");
      }
      else if ((s_left==0)&&(s_mleft==0)&&(s_mright==0)&&(s_right==1))         // if only sensor right detected black line
      {   forward();                                                // motor forward
         SPEEDL = 255;                                             // left motor speed is 255(full speed)
         SPEEDR = 0;                                                // right motor speed is 0
         memory = PORTB&0b00001111;
         lcd_goto(20);
         send_string ("left   ");
      }
      else if ((s_left==0)&&(s_mleft==0)&&(s_mright==0)&&(s_right==0))         // if all sensor coult not detected black line
      {   forward();                                                // motor forward
         if ((memory == 0b00000001)||(memory == 0b00000011)||(memory == 0b0000010)||(memory == 0b0000111))
         {
            SPEEDL = 0;                                             // left motor speed is 0
            SPEEDR = 255;                                          // right motor speed is 255(full speed)
         }
         else if ((memory == 0b00001000)||(memory == 0b0000100)||(memory == 0b00001100)||(memory == 0b0001110))
         {
            SPEEDL = 255;                                          // left motor speed is 255(full speed)
            SPEEDR = 0;                                             // right motor speed is 0
         }
      }
      else if ((s_left==1)&&(s_mleft==1)&&(s_mright==1)&&(s_right==1))         // if all sensor detected black line
      {
         forward();                                                // motor forward
      }
   }
}
//============================================================================================================
// Mode 2 : Ultrasonic
// Description: Maintain distance measure using ultrasonic between obsacle and robot
// Can choose data acquiring methode between ADC, PWM and UART
//============================================================================================================
void ultrasonic(void)      
{
   int distance;                                                // variable for distance measuring
   char n=1;                                                   // index for indicat mode
   lcd_clr();                                                   // clear lcd
   send_string("Measure mode");                                    // display string
   lcd_goto(20);                                                // lcd goto 2nd line
   send_string("ADC");                                             // Display string "ADC"

   while(1)                                                   // loop forever unless sw2 is pressed
   {
      if( !sw1)                                                // if button s1 is pressed
      {
         while(!sw1);                                          // wait until sw1 is release
         n++;                                                // increment n
         lcd_goto(20);                                          // goto 2nd line
         switch (n)                                             // check current value of n
         {
            case 2 :    send_string("PWM");            
                     break;                                    // break out from switch
            case 3 :    send_string("UART");                        
                     break;                                    // break out from switch   
            default:     send_string("ADC");                           // if not 2 or 3, set ti back to 1 and display "ADC"
                     n =1;
         }

      }
      
      if (!sw2)                                                // if button sw2 is pressed
      {
         while(!sw2);                                          // wait until sw2 is release
         break;                                                // break out form looping
      }
   }


   lcd_clr();                                                   // clear the lcd
   send_string("Distance");                                       // display string "Distance"

   while(1)                                                   // loop forever
   {
      lcd_goto(20);                                             // lcd goto 2nd line
      distance = us_value(n);                                       // disance variable are equal to value return from subroutine us_value
      dis_num(distance);                                          // display the value of distance


       if (distance> 40)                                          // check if distance more than 40
      {
         forward();                                             // then forward with full speed
         SPEEDL = 255;
         SPEEDR = 255;
         buzzer = 0;
      }
      else if (distance> 30)                                       // check if distance more than 40
      {
         forward();                                             // forward with medium speed
         SPEEDL = 230;
         SPEEDR = 230;
         buzzer = 0;
      }
      else if( distance >20)                                       // check if distance more than 40
      {
         stop();                                                // then stop
         buzzer = 0;
      }
      else                                                    // else, distance less than 20
      {
         backward();                                             // then backward with medium speed and on the buzzer
         SPEEDL = 230;
         SPEEDR = 230;
         buzzer = 1;
      }
   }

}

//============================================================================================================
// Mode 3 : Analog Distance Sensor
// Description : Maintain distance between robot and obstacle
//============================================================================================================
void analog_sen(void)
{
   int distance;                                                // variable for distance measuring
   lcd_clr();                                                   // clear lcd
   send_string("Distance");                                       // display string

   while(1)
   {
      lcd_goto(20);                                          // lcd goto 2nd line
      read_adc(CHANNEL1);                                       // read adc channel 1 ( analog distance sensor input)
      distance = result;                                       // assign distance as the result oft he reading
      dis_num(result);                                       // display the value   

      if (distance< 200)                                       // check if distance less than 200
      {
         backward();                                          // backward with full speed
         SPEEDL = 255;
         SPEEDR = 255;
         buzzer = 0;
      }
      else if (distance< 250)                                    // check if distance less than 250
      {
         backward();                                          // backward with medium speed
         SPEEDL = 230;
         SPEEDR = 230;
         buzzer = 0;
      }
      else if( distance < 300)                                 // check if distance less than 300
      {
         stop();                                             // stop
         buzzer = 0;
      }
      else                                                 // else, distance more than 300
      {
         forward();                                          //forward with medium speed and on buzzer
         SPEEDL = 230;
         SPEEDR = 230;
         buzzer = 1;
      }
   }
}

//============================================================================================================
//Mode 4 :  Xbee
// Description : Control the robot using UART ( XBEE or an UART wireless module.
//============================================================================================================
void wireless_xbee (void)      
{
   lcd_clr();                                                // clear the lcd
   while(1)                                                // looping forever
   {
      lcd_goto (0);                                 
      if (data[0] == 100)                                       // check if UART start byte is met
      {
         send_string("  XBEE CONTROL  ");                        // display string
         SPEEDL = 200;                                       // set the motor speed
         SPEEDR = 200;
         while(1)
         {
            lcd_goto(20);
            if (RCREG == '8')                                    // if character '8' is detected, the robot move forward
            {
               forward();
               send_string("FORWARD        ");
            }

            else if (RCREG == '2')                                 // if character '2' is detected, the robot move backward
            {
               backward();
               send_string("BACKWARD        ");
            }
            else if (RCREG == '6')                                 // if character '6' is detected, the robot turn right
            {
               right();
               send_string("TURN RIGHT      ");
            }

            else if (RCREG == '4')                                 // if character '4' is detected, the robot turn left
            {
               left();
               send_string("TURN LEFT       ");
            }

            else if (RCREG == '5')                                  // if character '5' is detected, then stop the robot
            {
               stop();
               send_string("INVALID COMMAND  ");
            }

            else                                              // else then stop the robot
            {
               stop();
               send_string("INVALID COMMAND  ");
            }
         }
      }
      else    send_string("COMMAND");
   }
}

//==================================================================================================
// Mode 5: SKPS
// Description: Control the robot using PS Controller
//==================================================================================================
void SKPS_PScon()
{
   unsigned char up_v, down_v, left_v, right_v;

   //Disabled Intterupt
   RCIE = 0;
   GIE = 0;
   PEIE = 0;
   TMR0IE = 0;   

   stop();         //stop all motor
   
   lcd_clr();
   lcd_goto(0);
   send_string("Controlling PR23");
   lcd_goto(20);
   send_string("   Using SKPS");
   
   while(1)
   {   
      //test button for horn
      if(skps(p_l1)==0)buzzer=1;         //if button L1 pressed, beep the buzzer
      else buzzer=0;                  
      
      //read joy stick value process      
      up_v=skps(p_joy_ru);
      down_v=skps(p_joy_rd);
      left_v=skps(p_joy_ll);
      right_v=skps(p_joy_lr);      
            
      //button control for mobilility
      if(skps(p_up)==0)               //check "up" button
      {
         forward();                  //move forward
         SPEEDL=230;
         SPEEDR=230;
      }
      else if(skps(p_down)==0)         //check "down" button
      {   
         backward();                  //move backward
         SPEEDL=230;
         SPEEDR=230;
      }
      else if(skps(p_left)==0)         //check "left" button
      {
         left();                     //rotate left
         SPEEDL=230;
         SPEEDR=230;
      }
      else if(skps(p_right)==0)         //check "right" button
      {
         right();                  //rotate right
         SPEEDL=230;
         SPEEDR=230;
      }
            
      //analog control for mobility   
      else if(up_v>0)
      {
         forward();
         if(left_v>0)
         {
            if(up_v>left_v)SPEEDL=up_v-left_v+140;
            else SPEEDL=140;
            SPEEDR=up_v+140;      
         }
         else if(right_v>0)
         {
            if(up_v>right_v)SPEEDR=up_v-right_v+140;
            else SPEEDR=140;
            SPEEDL=up_v+140;
         }
         else
         {
            SPEEDL=up_v+140;
            SPEEDR=up_v+140;   
         }
      }
      else if(down_v>0)
      {
         backward();
         if(left_v>0)
         {
            if(down_v>left_v)SPEEDR=down_v-left_v+140;
            else SPEEDR=140;
            SPEEDL=down_v+140;      
         }
         else if(right_v>0)
         {
            if(down_v>right_v)SPEEDL=down_v-right_v+140;
            else SPEEDL=140;
            SPEEDR=down_v+140;
         }
         else
         {
            SPEEDL=down_v+140;
            SPEEDR=down_v+140;   
         }
      }
      else if(left_v>0)
      {
         left();
         SPEEDL=left_v+120;
         SPEEDR=left_v+120;   
      }
      else if(right_v>0)
      {
         right();
         SPEEDL=right_v+120;
         SPEEDR=right_v+120;   
      }
      else
      {
         stop();
         SPEEDL=255;
         SPEEDR=255;   
      }
   }   
}

//====================================================================================================
// Ultrasonic value
// Description : Retrive data from Ultrsonic. Can choose methode between ADC, PWM and UART
// Parameter : mode      1) using analog
//                  2) using pwm
//                  3) using uart
//====================================================================================================
unsigned int us_value (unsigned char mode)                                 // subroutine for ultrasonic measurement
{
   unsigned int value;
   switch (mode)                                                   // retrive value of measured distane based on the methode selected
   {
      case 1:   read_adc(CHANNEL0);
            value = result;                                          // max vslue 2.55v = 2.55/5 *1024 - 1 =  522, resolution = 10mV/ inch, 10m/5*1024 =~ 2
            break;
      case 2:   value = TH;                                             // each value = 256*4/20mhz = 51.2us, i inch = 147us 
            break;                                                // can change using smaller timer prescale, but resulation fixed 147us / inch
      case 3:   if ( data [0]=='R')   value = (data[1] - 0x30)*100+ (data[2] - 0x30)*10+ (data[3] - 0x30); // 1 = 1 inch
            else
            {   
               lcd_goto(20);                                       // if stater byte is not 'R', Display 'not connected'
               send_string("not connected");
               while(1);                                          // loop forever
            }   
      default: ;
   }
   return value;
}   


//===========================================================================================================
// read adc
// Description: subroutine for converting analog value to digital with average 200 samples
// Parameter : config  ( select the channel )
//============================================================================================================
void read_adc(char config)
{
   unsigned short i;
   unsigned long result_temp=0;

   ADCON0 = config;
   delay(10000);               // delay after changing configuration
   for(i=200;i>0;i-=1)            //looping 200 times for getting average value
   {
      ADGO = 1;               //ADGO is the bit 2 of the ADCON0 register
      while(ADGO==1);            //ADC start, ADGO=0 after finish ADC progress
      result=ADRESH;
      result=result<<8;         //shift to left for 8 bit
      result=result|ADRESL;      //10 bit result from ADC

      result_temp+=result;      
   }
   result = result_temp/200;      //getting average value
   ADON = 0;                  //adc module is shut off
}


//===========================================================================================================
// Motor control function
// Description : subroutine to set the robot moving direction
//============================================================================================================
void forward ()
{
   motor_ra = 0;
   motor_rb = 1;
   motor_la = 0;
   motor_lb = 1;
}

void backward ()
{
   motor_ra = 1;
   motor_rb = 0;
   motor_la = 1;
   motor_lb = 0;
}

void left()
{
   motor_la = 1;
   motor_lb = 0;
   motor_ra = 0;
   motor_rb = 1;
}
void right()
{
   motor_la = 0;
   motor_lb = 1;
   motor_ra = 1;
   motor_rb = 0;
}

void stop()
{
   motor_la = 1;
   motor_lb = 1;
   motor_ra = 1;
   motor_rb = 1;
}

//===========================================================================================================
//   LCD   functions
//============================================================================================================
void delay(unsigned long data)               //delay function, the delay time
{
   for( ;data>0;data-=1);                  //depend on the given value
}

void send_config(unsigned char data)         //send lcd configuration
{
   rs=0;                              //set lcd to config mode
   lcd_data=data;                        //lcd data port = data
   delay(400);                        
   e_pulse();                           //pulse e to confirm the data
}

void send_char(unsigned char data)            //send lcd character
{
   rs=1;                              //set lcd to display mode
   lcd_data=data;                        //lcd data port = data            
   delay(400);
   e_pulse();                           //pulse e to confirm the data
}

void e_pulse(void)                        //pulse e to confirm the data
{
   e=1;
   delay(300);
   e=0;
   delay(300);
}

void lcd_goto(unsigned char data)            //set the location of the lcd cursor
{
    if(data<16)                           //if the given value is (0-15) the
   {                                 //cursor will be at the upper line
       send_config(0x80+data);
   }
   else                              //if the given value is (20-35) the
   {                                 //cursor will be at the lower line
       data=data-20;                     //location of the lcd cursor(2X16):
      send_config(0xc0+data);               // -----------------------------------------------------
   }                                 // | |00|01|02|03|04|05|06|07|08|09|10|11|12|13|14|15| |
}                                    // | |20|21|22|23|24|25|26|27|28|29|30|31|32|33|34|35| |
                                    // -----------------------------------------------------   

void lcd_clr(void)                        //clear the lcd
{
    send_config(0x01);
   delay(350);   
}

void send_string(const char *s)             //send a string to display in the lcd
{         
     while (s && *s)send_char (*s++);
}

void dis_num(unsigned long data)
{
   unsigned char hundred_thousand;
   unsigned char ten_thousand;
   unsigned char thousand;
   unsigned char hundred;
   unsigned char tenth;

   hundred_thousand = data/100000;                  // devide to get the numerator       eg: 5234/1000 = 5
   data = data % 100000;                        // modulas to get the remainder    eg: 5234%1000 = 234
   ten_thousand = data/10000;
   data = data % 10000;
   thousand = data / 1000;
   data = data % 1000;
   hundred = data / 100;
   data = data % 100;
   tenth = data / 10;
   data = data % 10;

   send_char(hundred_thousand + 0x30);               //0x30 added to become ASCII code char '0' to '9'
   send_char(ten_thousand + 0x30);
   send_char(thousand + 0x30);
   send_char(hundred + 0x30);
   send_char(tenth + 0x30);
   send_char(data + 0x30);
}

//====================================================================================================
// uart function
//====================================================================================================
void uart_send(unsigned char data)   //function to send out a byte via uart
{   
   while(TXIF==0);               //wait for previous data to finish send out
   TXREG=data;                  //send new data
}

unsigned char uart_rec(void)      //function to wait for a byte receive from uart
{
   unsigned char temp;
   while(RCIF==0);               //wait for data to received
   temp=RCREG;               
   return temp;               //return the received data
}

//==================================================================================================
// skps function
//==================================================================================================
unsigned char skps(unsigned char data)         //function to read button and joystick
{                                    //information on ps controller
   uart_send(data);
   return uart_rec();
}

void skps_vibrate(unsigned char motor, unsigned char value)
{                                    //function to control the vibrator motor
   uart_send(motor);                     //on ps controller
   uart_send(value);   
}
dausvilla77
Greenhorn
 
Posts: 3
Joined: Thu Nov 28, 2013 1:48 am

Re: Multifunction Mobile Robot Rev2

Postby ober » Thu Nov 28, 2013 11:46 am

Basically, you will need to change this function:

CODE: SELECT_ALL_CODE
// Mode 1 : line follow subroutine
// Description: Program for the robot to follow line
// For more detail about line follow concept please refer PR5
//============================================================================================================
void line_follow()                     
{
   unsigned char memory;

   lcd_clr();                        // clear lcd screen
   send_string("Position");            // display "position" string


   while(1)
   {
      if ((s_left==1)&&(s_mleft==0)&&(s_mright==0)&&(s_right==0))              // if only sensor left detected black line
      {   forward();                                                // motor forward
         SPEEDL = 0;                                                // left motor speed is 0
         SPEEDR = 255;                                             // right motor speed is 255(full speed)
         memory = PORTB&0b00001111;                                    // save current sensor position
         lcd_goto(20);                                             // lcd go to 2nd line 1st character
         send_string ("right  ");                                    // display "right"mean the robot's position is on the right side of the line
      }
      else if ((s_left==1)&&(s_mleft==1)&&(s_mright==0)&&(s_right==0))         // if only sensor left detected black line
      {   forward();                                                // motor forward
         SPEEDL = 180;                                             // left motor speed is 180
         SPEEDR = 255;                                             // right motor speed is 255(full speed)
         memory = PORTB&0b00001111;
         lcd_goto(20);
         send_string ("m_right2");
      }
      else if ((s_left==0)&&(s_mleft==1)&&(s_mright==0)&&(s_right==0))         // if only sensor middle left detected black line
      {   forward();                                                // motor forward
         SPEEDL = 200;                                             // left motor speed is 200
         SPEEDR = 255;                                             // right motor speed is 255(full speed)
         memory = PORTB&0b00001111;
         lcd_goto(20);
         send_string ("m_right1  ");
      }
      else if ((s_left==1)&&(s_mleft==1)&&(s_mright==1)&&(s_right==0))         // if sensor middle left and sensor left detected black line
      {   forward();                                                // motor forward
         SPEEDL = 200;                                             // left motor speed is 200
         SPEEDR = 255;                                             // right motor speed is 255(full speed)
         memory = PORTB&0b00001111;
         lcd_goto(20);
         send_string ("m_right1  ");
      }
      else if ((s_left==0)&&(s_mleft==1)&&(s_mright==1)&&(s_right==0))         // if sensor middle left and sensor middle right detected black line
      {   forward();                                                // motor forward
         SPEEDL = 255;                                             // left motor speed is 255(full speed)
         SPEEDR = 255;                                             // right motor speed is 255(full speed)
         memory = PORTB&0b00001111;
         lcd_goto(20);
         send_string ("middle ");
      }
      else if ((s_left==0)&&(s_mleft==0)&&(s_mright==1)&&(s_right==0))         // if only sensor middle right detected black line
      {   forward();                                                // motor forward
         SPEEDL = 255;                                             // left motor speed is 255(full speed)
         SPEEDR = 200;                                             // right motor speed is 200
         memory = PORTB&0b00001111;
         lcd_goto(20);
         send_string ("m_left1   ");
      }
      else if ((s_left==0)&&(s_mleft==1)&&(s_mright==1)&&(s_right==1))         // if sensor middle left, sensor middle right and sensor right detected black line
      {   forward();                                                // motor forward
         SPEEDL = 255;                                             // left motor speed is 255(full speed)
         SPEEDR = 200;                                             // right motor speed is 200
         memory = PORTB&0b00001111;
         lcd_goto(20);
         send_string ("m_left1   ");
      }
      else if ((s_left==0)&&(s_mleft==0)&&(s_mright==1)&&(s_right==1))         // if sensor right and sensor middle right detected black line
      {   forward();                                                // motor forward
         SPEEDL = 255;                                             // left motor speed is 255(full speed)
         SPEEDR = 180;                                             // right motor speed is 180
         memory = PORTB&0b00001111;
         lcd_goto(20);
         send_string ("m_left2 ");
      }
      else if ((s_left==0)&&(s_mleft==0)&&(s_mright==0)&&(s_right==1))         // if only sensor right detected black line
      {   forward();                                                // motor forward
         SPEEDL = 255;                                             // left motor speed is 255(full speed)
         SPEEDR = 0;                                                // right motor speed is 0
         memory = PORTB&0b00001111;
         lcd_goto(20);
         send_string ("left   ");
      }
      else if ((s_left==0)&&(s_mleft==0)&&(s_mright==0)&&(s_right==0))         // if all sensor coult not detected black line
      {   forward();                                                // motor forward
         if ((memory == 0b00000001)||(memory == 0b00000011)||(memory == 0b0000010)||(memory == 0b0000111))
         {
            SPEEDL = 0;                                             // left motor speed is 0
            SPEEDR = 255;                                          // right motor speed is 255(full speed)
         }
         else if ((memory == 0b00001000)||(memory == 0b0000100)||(memory == 0b00001100)||(memory == 0b0001110))
         {
            SPEEDL = 255;                                          // left motor speed is 255(full speed)
            SPEEDR = 0;                                             // right motor speed is 0
         }
      }
      else if ((s_left==1)&&(s_mleft==1)&&(s_mright==1)&&(s_right==1))         // if all sensor detected black line
      {
         forward();                                                // motor forward
      }
   }
}


Make the logic of all sensor (s_left, s_mleft, s_mright) into inverse, if it is logic 0, change to 1 and vise versa. Please change all the condition, Not to forget the memory too.

Please do study and understand the code, you need to understand the concept to be able to modify and make use of it.
Ober Choo
Cytron Technologies Sdn Bhd
www.cytron.com.my
User avatar
ober
Moderator
 
Posts: 1486
Joined: Wed Apr 15, 2009 1:03 pm

Re: Multifunction Mobile Robot Rev2

Postby dausvilla77 » Thu Nov 28, 2013 2:04 pm

Thank you very much, Ober..I has edited the code as you said and it can be build. Can you check, is it like this ?

CODE: SELECT_ALL_CODE
/============================================================================================================
// Mode 1 : line follow subroutine
// Description: Program for the robot to follow line
// For more detail about line follow concept please refer PR5
//============================================================================================================
void line_follow()                     
{
   unsigned char memory;

   lcd_clr();                        // clear lcd screen
   send_string("Position");            // display "position" string


   while(1)
   {
      if ((s_left==0)&&(s_mleft==1)&&(s_mright==1)&&(s_right==1))              // if only sensor left detected white line
      {   forward();                                                // motor forward
         SPEEDL = 0;                                                // left motor speed is 0
         SPEEDR = 255;                                             // right motor speed is 255(full speed)
         memory = PORTB&0b11110000;                                    // save current sensor position
         lcd_goto(20);                                             // lcd go to 2nd line 1st character
         send_string ("right  ");                                    // display "right"mean the robot's position is on the right side of the line
      }
      else if ((s_left==0)&&(s_mleft==0)&&(s_mright==1)&&(s_right==1))         // if only sensor left detected white line
      {   forward();                                                // motor forward
         SPEEDL = 180;                                             // left motor speed is 180
         SPEEDR = 255;                                             // right motor speed is 255(full speed)
         memory = PORTB&0b11110000;
         lcd_goto(20);
         send_string ("m_right2");
      }
      else if ((s_left==1)&&(s_mleft==0)&&(s_mright==1)&&(s_right==1))         // if only sensor middle left detected white line
      {   forward();                                                // motor forward
         SPEEDL = 200;                                             // left motor speed is 200
         SPEEDR = 255;                                             // right motor speed is 255(full speed)
         memory = PORTB&0b11110000;
         lcd_goto(20);
         send_string ("m_right1  ");
      }
      else if ((s_left==0)&&(s_mleft==0)&&(s_mright==0)&&(s_right==1))         // if sensor middle left and sensor left detected white line
      {   forward();                                                // motor forward
         SPEEDL = 200;                                             // left motor speed is 200
         SPEEDR = 255;                                             // right motor speed is 255(full speed)
         memory = PORTB&0b11110000;
         lcd_goto(20);
         send_string ("m_right1  ");
      }
      else if ((s_left==1)&&(s_mleft==0)&&(s_mright==0)&&(s_right==1))         // if sensor middle left and sensor middle right detected white line
      {   forward();                                                // motor forward
         SPEEDL = 255;                                             // left motor speed is 255(full speed)
         SPEEDR = 255;                                             // right motor speed is 255(full speed)
         memory = PORTB&0b11110000;
         lcd_goto(20);
         send_string ("middle ");
      }
      else if ((s_left==1)&&(s_mleft==1)&&(s_mright==0)&&(s_right==1))         // if only sensor middle right detected white line
      {   forward();                                                // motor forward
         SPEEDL = 255;                                             // left motor speed is 255(full speed)
         SPEEDR = 200;                                             // right motor speed is 200
         memory = PORTB&0b11110000;
         lcd_goto(20);
         send_string ("m_left1   ");
      }
      else if ((s_left==1)&&(s_mleft==0)&&(s_mright==0)&&(s_right==0))         // if sensor middle left, sensor middle right and sensor right detected white line
      {   forward();                                                // motor forward
         SPEEDL = 255;                                             // left motor speed is 255(full speed)
         SPEEDR = 200;                                             // right motor speed is 200
         memory = PORTB&0b11110000;
         lcd_goto(20);
         send_string ("m_left1   ");
      }
      else if ((s_left==1)&&(s_mleft==1)&&(s_mright==0)&&(s_right==0))         // if sensor right and sensor middle right detected white line
      {   forward();                                                // motor forward
         SPEEDL = 255;                                             // left motor speed is 255(full speed)
         SPEEDR = 180;                                             // right motor speed is 180
         memory = PORTB&0b11110000;
         lcd_goto(20);
         send_string ("m_left2 ");
      }
      else if ((s_left==1)&&(s_mleft==1)&&(s_mright==1)&&(s_right==0))         // if only sensor right detected white line
      {   forward();                                                // motor forward
         SPEEDL = 255;                                             // left motor speed is 255(full speed)
         SPEEDR = 0;                                                // right motor speed is 0
         memory = PORTB&0b11110000;
         lcd_goto(20);
         send_string ("left   ");
      }
      else if ((s_left==1)&&(s_mleft==1)&&(s_mright==1)&&(s_right==1))         // if all sensor coult not detected white line
      {   forward();                                                // motor forward
         if ((memory == 0b10000000)||(memory == 0b11111100)||(memory == 0b1111101)||(memory == 0b1111000))
         {
            SPEEDL = 0;                                             // left motor speed is 0
            SPEEDR = 255;                                          // right motor speed is 255(full speed)
         }
         else if ((memory == 0b11110111)||(memory == 0b1111011)||(memory == 0b11110011)||(memory == 0b1110001))
         {
            SPEEDL = 255;                                          // left motor speed is 255(full speed)
            SPEEDR = 0;                                             // right motor speed is 0
         }
      }
      else if ((s_left==0&&(s_mleft==0&&(s_mright==0&&(s_right==0)         // if all sensor detected white line
      {
         forward();                                                // motor forward
      }
   }
}

dausvilla77
Greenhorn
 
Posts: 3
Joined: Thu Nov 28, 2013 1:48 am


Return to DIY Project Set

Who is online

Users browsing this forum: No registered users and 13 guests

cron