Skip to content

Instantly share code, notes, and snippets.

Embed
What would you like to do?
Buggy line follower with MikroC for PIC code
/*******************************************************************
____ __ ____ ___ ____ ____ __ __ _ ____ __
( __)( ) ( __)/ __)(_ _)( _ \ / \ ( ( \(__ ) / _\
) _) / (_/\ ) _)( (__ )( ) /( 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);
}
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment