Buggy line follower with MikroC for PIC code

line_follower.c

/*******************************************************************
 ____  __    ____  ___  ____  ____   __   __ _  ____   __  
(  __)(  )  (  __)/ __)(_  _)(  _ \ /  \ (  ( \(__  ) / _\ 
 ) _) / (_/\ ) _)( (__   )(   )   /(  O )/    / / _/ /    \
(____)\____/(____)\___) (__) (__\_) \__/ \_)__)(____)\_/\_/

Project name:  Mikroe Buggy: Line follower with MikroC for PIC code
Project page: https://electronza.com/mikroe-buggy-line-follower-mikroc-pic18/
Description:  MikroC code
********************************************************************/

/*******************************************************************************
* Globals
*******************************************************************************/

// will be used by motor logic
#define KP .5 //
#define KD 5
#define DEFAULT_SPEED 60
#define MAX_SPEED 150
#define MIN_SPEED 20
int error;
int lasterror;
int motorSpeed;
int leftMotorSpeed;
int rightMotorSpeed;
unsigned int PWM_A_PERIOD  = 255;
unsigned int PWM_B_PERIOD  = 255;
unsigned int PWM_C_PERIOD  = 255;
unsigned int PWM_D_PERIOD  = 255;

// =============================================================================
// Pin definitions for LEDS on the Clicker 2

// LEDs
sbit LED1 at LATD4_bit;
sbit LED2 at LATE4_bit;
sbit LED1_Direction at TRISD4_bit;
sbit LED2_Direction at TRISE4_bit;
//Switches
sbit SW1 at RD7_bit;
sbit SW2 at RH3_bit;
sbit SW1_Direction at TRISD7_bit;
sbit SW2_Direction at TRISH3_bit;

// =============================================================================
// Pin definitions for LEDS on the BUGGY
// Lights
sbit Brakelights at LATJ6_bit;
sbit Headlights at LATB5_bit;
sbit SignalL at LATF5_bit;
sbit SignalR at LATJ4_bit;
sbit MainBeam at LATJ3_bit;
sbit Brake_Direction at TRISJ6_bit;
sbit Headlights_Direction at TRISB5_bit;
sbit SignalL_Direction at TRISF5_bit;
sbit SignalR_Direction at TRISJ4_bit;
sbit MainBeam_direction at TRISJ3_bit;

// Motors

// Line sensors
sbit SLeft_dir at TRISH7_bit;
sbit SMiddle_dir at TRISF2_bit;
sbit SRight_dir at TRISH5_bit;
// =============================================================================

// Some variables

int idx;
long int readout_val[3];
int Sensor_value; // value from ADC
int ADCchannel[3];
int Line_reading[3];
long int position; // this will be used for driving logic
long int sum;
int denominator;
int temp;

// The following variables are used to dynamically adjust the scaling factors
int ONline = 200;
int OFFline = 100;
int MinCal[3];
int MaxCal[3];

/*******************************************************************************
* Functions for the line sensors
*******************************************************************************/
/* This functiona are heavily inspired from Pololu QTR Arduino code
 *
 * Pololu ciode written by Ben Schmidel et al., October 4, 2010.
 * Copyright (c) 2008-2012 Pololu Corporation. For more information, see
 *
 *   http://www.pololu.com
 *   http://forum.pololu.com
 *   http://www.pololu.com/docs/0J19
 *
 * You may freely modify and share this code, as long as you keep this
 * notice intact (including the two links above).  Licensed under the
 * Creative Commons BY-SA 3.0 license:
 *
 *   http://creativecommons.org/licenses/by-sa/3.0/
 *
 * Disclaimer: To the extent permitted by law, Pololu provides this work
 * without any warranty.  It might be defective, in which case you agree
 * to be responsible for all resulting costs and damages.
 */

void resetCalibration(){
  for (idx = 1; idx <4; idx ++){
    MinCal[idx] = 0;
    MaxCal[idx] = 1024;
  }
}

void readout(){
   //idx = 1;
   for (idx = 0; idx <3; idx ++){
   Sensor_value = ADC_Read(ADCchannel[idx]);
   // compute new scaling factors
   if (Sensor_value > ONline){ // we are definetely on the line
     if (Sensor_value > MaxCal[idx]){
        MaxCal[idx] = Sensor_value;
     }
     else {
        temp = (MaxCal[idx] - Sensor_value) * 2 /3;
        MaxCal[idx] = temp + Sensor_value;
     }
   }
   // one more check
   if (MaxCal[idx] < 120){  //sometimes shit happens (white reading)
     MaxCal[idx] = 120;
   }
   if (MaxCal[idx] >1024){
     MaxCal[idx] = 1024;
   }
   if (Sensor_value < OFFline){ // we are definetely on the line
     if (Sensor_value < MinCal[idx]){
        MinCal[idx] = Sensor_value;
     }
     else {
        temp = (Sensor_Value - MinCal[idx]) * 1 /3;
        MinCal[idx] = temp + MinCal[idx];
     }
   }
   // one more check
   if (MinCal[idx] > 70){  //sometimes shit happens (black reading)
     MinCal[idx] = 70;
   }
   if (MinCal[idx] < 0 ) {
     MinCal[idx] = 0;
   }

   // compute sensor scaling
   denominator = MaxCal[idx] - MinCal[idx];
   readout_val[idx] = (long int)(Sensor_Value - MinCal[idx]) * 1000 / denominator;
   }

   // now we compute the position
   position = (long int) readout_val[0] + 1000 * readout_val[1] + 2000 * readout_val[2];
   sum = (long int) readout_val[0] + readout_val[1] + readout_val[2];
   position = (long int)position / sum;
   if (position < 0){
     position = 0;
   }
   if (position > 2000){
     position = 2000;
   }
}

/*******************************************************************************
* Functions to drive the motors
*******************************************************************************/
void Break_Buggy() {
  leftMotorSpeed = 0;
  rightMotorSpeed = 0;
  PWM1_Start();                       // start PWM1
  PWM2_Start();                       // start PWM2
  PWM3_Start();                       // start PWM3
  PWM5_Start();                       // start PWM5
  PWM1_Set_Duty(0);
  PWM2_Set_Duty(0);
  PWM5_Set_Duty(0);
  PWM3_Set_Duty(0);
}

void Drive_Buggy(int speedL, int speedR) {
   // LEFT MOTORS
   PWM1_Set_Duty(speedL);
   PWM2_Set_Duty(0);
   // RIGHT MOTORS
   PWM3_Set_Duty(speedR);
   PWM5_Set_Duty(0);
}


void main(){

// set pins for lights as digital outputs
SignalL_Direction = 0;
SignalR_Direction = 0;
MainBeam_direction = 0;
Headlights_Direction = 0;

// set pins for sensors as inputs
SLeft_dir = 1;
SMiddle_dir = 1;
SRight_dir = 1;

// configure ADC
ANCON0 = 0x1F; // only AN7 is set as analog input
ANCON1 = 0x5C; // AN13 and AN15 configured as analog inputs
Adc_init();
ADCchannel[0] = 15;
ADCchannel[1] = 7;
ADCchannel[2] = 13;

// turn on main beam
SignalR = 0;
SignalL = 0;
MainBeam = 0;
Headlights = 1;

// init the motors
PWM1_Init(5000);
PWM2_Init(5000);                    //PWM_A_PERIOD;
PWM5_Init(5000);
PWM3_Init(5000);

PWM1_Start();                       // start PWM1
PWM2_Start();                       // start PWM2
PWM3_Start();                       // start PWM3
PWM5_Start();                       // start PWM5
PWM1_Set_Duty(0);
PWM2_Set_Duty(0);
PWM5_Set_Duty(0);
PWM3_Set_Duty(0);
Break_Buggy();

// I don't want the robot to start running the moment I apply power
delay_ms(5000);

// drive at full speed for 200 ms
// to overcome any inertial forces
Drive_Buggy(255,255);
delay_ms(200);



  while(1){
    readout();
     // turn lights logic
     if (position < 750){
         SignalR = 0;
         SignalL = 1;
         MainBeam = 0;
     }
     else if (position < 1250){
         SignalR = 0;
         SignalL = 0;
         MainBeam = 1;
     }
     else    {
         SignalR = 1;
         SignalL = 0;
         MainBeam = 0;
     }

    // adjust the motors
    error = (int) position - 1000;
    motorSpeed = (int) (KP * error + KD * (error - lastError));
    lastError = error;
    leftMotorSpeed = DEFAULT_SPEED + motorSpeed;
    rightMotorSpeed = DEFAULT_SPEED - motorSpeed;

    if (leftMotorSpeed > MAX_SPEED ){
       leftMotorSpeed = MAX_SPEED; // limit top speed
    }
    if (leftMotorSpeed > MAX_SPEED ){
       leftMotorSpeed = MAX_SPEED; // limit top speed
    }
    if (rightMotorSpeed < MIN_SPEED){
       rightMotorSpeed = MIN_SPEED; // keep motor above 0
    }
    if (leftMotorSpeed < MIN_SPEED) {
       leftMotorSpeed = MIN_SPEED; // keep motor speed above 0
    }
    Drive_Buggy(leftMotorSpeed,rightMotorSpeed);
    delay_ms(50);
  }
}