 |
 |
| View previous topic :: View next topic |
| Author |
Message |
srikrishna
Joined: 06 Sep 2017
Posts: 82
|
| Line follower using analog sensor |
 Posted: Wed Sep 06, 2017 9:11 pm |
 |
|
Hello guys this is my line follower code using analog sensor, although it is not completed yet. If there is any error please check it and inform me.
| Code: |
#include <18F2550.h>
#device ADC=10
#include <math.h>
#include <stdlib.h>
#include <stdio.h>
#FUSES HS,NOWDT
#define MOTORLEFT_DIR_A PIN_B0
#define MOTORLEFT_DIR_B PIN_B1
#define MOTORRIGH_DIR_A PIN_B2
#define MOTORRIGH_DIR_B PIN_B4
#define NUM_SENSORS 5 // number of sensors used
#USE delay(clock=20M)
#use rs232(baud=9600, xmit=PIN_C6, rcv=PIN_C7,PARITY=N,BITS=8)
#USE TIMER(TIMER=1,TICK=1s,BITS=16,NOISR)
int32 derrivative,proportional,last_prportional,power_difference; // PID LOOP Variables
float kp,kd;
int16 setPoint = ((NUM_SENSORS-1)*1000)/2;
unsigned int16 tick_difference(unsigned int16 current,unsigned int16 previous)
{
return (current - previous);
}
double map(double value, float min, float max, float y_min, float y_max)//function definition
{
return (y_min + (((y_max - y_min)/(max - min)) * (value - min)));
}
typedef struct
{
unsigned int16 min[5];
unsigned int16 max[5];
} calibration_t; //calibration_t is the new name of our structure definition
void calibrate_adc(calibration_t *calibration_data)//calibration_t is structure variable
{
unsigned int16 current_tick;
unsigned int16 starting_tick;
unsigned int16 sensor_value[]={0,0,0,0,0};
unsigned int16 max[] = {0,0,0,0,0};
unsigned int16 min[] = {1023,1023,1023,1023,1023};
unsigned int8 i;
delay_us(15);
starting_tick = current_tick = get_ticks();
current_tick = get_ticks();
//get as many readings as you can. the more the
//merrier
while(tick_difference(current_tick,starting_tick) < 5)
{
for (i=0;i<5;i++)
{
set_adc_channel(i);
delay_us(15);
sensor_value[i] = read_adc();
if(sensor_value[i] > max[i])
{
max[i] = sensor_value[i];
}
if(sensor_value[i] < min[i])
{
min[i] = sensor_value[i];
}
output_toggle(PIN_B0);
current_tick = get_ticks();
}
calibration_data->min[i] = min[i]; //a->b is just short for (*a).b in every way (same for functions: a->b() is short for (*a).b()).
calibration_data->max[i] = max[i]; //(*calibration_data).max,
//(*calibration_data).min = min[i];//a->b is equivalent to (*a).b, i.e. it gets the member called b from the struct that a points to.
//(*calibration_data).max = max[i];
}
}
void setmotor_left( int16 value)
{
if (value >= 0)
{
output_high(MOTORRIGH_DIR_A);
output_low(MOTORRIGH_DIR_B);
}
else
{
output_low(MOTORRIGH_DIR_A);
output_high(MOTORRIGH_DIR_B);
value *= -1 ;
}
set_pwm1_duty(value);
}
void setmotor_right( int16 value )
{
if (value >=0)
{
output_high(MOTORLEFT_DIR_A);
output_low(MOTORLEFT_DIR_B);
}
else
{
output_low(MOTORLEFT_DIR_A);
output_high(MOTORLEFT_DIR_B);
value *= -1 ;
}
set_pwm2_duty(value);
}
void setmotors( int16 left , int16 right )
{
setmotor_left( left );
setmotor_right( right );
}
void main(void)
{
calibration_t cal;// structure varible
unsigned int16 sensor_value[]={0,0,0,0,0};
double map_result[]={0,0,0,0,0};
unsigned int8 i;
enable_interrupts(INT_RDA); //Serial Interrupt Enable
enable_interrupts(GLOBAL); //Global Interrupt Enable
setup_ccp1(CCP_PWM); // CCP1 Initialization pin12 of 18f2550
setup_ccp2(CCP_PWM); // CCP2 Initialization pin 13 of 18f2550
setup_timer_2(T2_DIV_BY_16, 250, 1);
power_difference = 0;
derrivative = 0;
proportional = 0;
last_prportional = 0;
int16 max = 162;// variable to control pwm
SETUP_ADC_PORTS( AN0_TO_AN4_ANALOG);
setup_adc(ADC_CLOCK_DIV_32);
int32 sensors_average = 0;
int32 sensors_sum = 0;
unsigned int32 line_position=0;
delay_ms(1000); //some delay to stabilize sensor
calibrate_adc(&cal); //takes about 5 secs
//next two lines not technically needed
//since you only have one ADC and it
//was set in calibrate_adc(), but here
//for later expansion
delay_us(15);
while(1)
{
for (i=0;i<5;i++)
{
set_adc_channel(i);
delay_us(15);
sensor_value[i] = read_adc();
map_result[i] = map(sensor_value[i],cal.min[i],cal.max[i],0,1023);
//printf("%f \r\n",map_result[i]);
//delay_ms(1000);
delay_us(2);
sensors_average += (map_result[i] * (i) * 1000); //Calculating the weighted mean
sensors_sum += map_result[i];
line_position = (sensors_average / sensors_sum);
proportional = (line_position) - setPoint;
derrivative = proportional - last_prportional;
last_prportional= proportional;
power_difference = (proportional*Kp) + (last_prportional*kd);
if ( power_difference > max )
{
power_difference = max;
}
else if( power_difference < -max )
{
power_difference = -max;
}
// Assign calculated speed to the differential power of the motors
(power_difference < 0 ) ? setmotors(max+power_difference, max) : setmotors(max, max-power_difference);
}
}
}
|
Last edited by srikrishna on Mon Nov 13, 2017 11:51 pm; edited 3 times in total |
|
 |
Ttelmah
Joined: 11 Mar 2010
Posts: 19520
|
|
| Posted: Thu Sep 07, 2017 1:31 am |
|
|
Only looked at a few bits, but picked up the following:
| Code: |
#include <18F2550.h>
#device adc=10
#fuses NOWDT,PUT,HS,PROTECT
#use delay(clock=20M)
//You need to tell it _how_ this clock is being generated
//Either:
#use delay(crystal=20MHz, clock=20MHz)
//Or
#fuses NOWDT,PUT,HS,PROTECT, CPUDIV1
|
Currently you are not telling it how to generate the '20Mhz' from your clock source....
This assumes you have got a 20MHz crystal attached. It isn't going to work unless you have....
Then:
| Code: |
setup_adc(ADC_CLOCK_INTERNAL );// ADC Clock Internal
|
ADC_CLOCK_INTERNAL, is _not_ recommended on this PIC above 1MHz clock, unless you are stopping the CPU to perform the conversion. Look at the data sheet.
At 20MHz, use ADC_CLOCK_DIV_16
Then You either need to program an acquisition time between selecting an ADC channel (ADC_TAD_MUL_4), or pause for about 7uSec between selecting the channel and reading.
Then you have set the CCP's up to be PWM's, but have not programmed the timer needed to feed the PWM's....
I didn't look at the main code. |
|
|
temtronic
Joined: 01 Jul 2010
Posts: 9229
Location: Greensville,Ontario
|
|
| Posted: Thu Sep 07, 2017 5:07 am |
|
|
The group of defines should be AFTER the processor include and fuses code.
Can't have kd=17.5 when kd is an integer.
You should never use 'protect' until the day the code is 100% and product is ready to ship.
this...
| Quote: | | sensors_average += qre[i]*i*1000; |
just doesn't look right to me to get an average.
Though perhaps you're using a 'linear bar of sensors' in the front of the car, sensor 0-1-2-3-4, so sensor2 is 'dead center', 4 is off right side ?
Jay |
|
|
srikrishna
Joined: 06 Sep 2017
Posts: 82
|
|
| Posted: Thu Sep 07, 2017 3:04 pm |
|
|
I am using a sensor array, but the problem is how to determine the line position. I use
| Code: | for (i=0;i<5;i++)
{
set_adc_channel(i);
qre[i]= read_adc();
sensors_average += qre[i]*i*1000;
sensors_sum += qre[i];
}
while(1)
{
line_position = (sensors_average / sensors_sum);
|
But in Pololu 3pi robot they used:
| Code: |
unsigned int position = robot.readLine(sensors, IR_EMITTERS_ON); |
For qtr line sensor in arduino,
| Code: |
line_position = qtrrc.readLine(sensorValues);
|
to determine the line position, so what function should i use in ccs c?? |
|
|
temtronic
Joined: 01 Jul 2010
Posts: 9229
Location: Greensville,Ontario
|
|
| Posted: Thu Sep 07, 2017 4:44 pm |
|
|
To answer, you must post the make/model/product information for the 'sensor'. We can only make guesses as to what it might be, how to use it, and how to cut CCS C code.
The code fragments you show are really 'functions'. We'd need to see the ACTUAL code within those functions to see what is happening. The 'real' code may not be available unless they are 'open sourced'.
Jay |
|
|
srikrishna
Joined: 06 Sep 2017
Posts: 82
|
|
| Posted: Thu Sep 07, 2017 5:06 pm |
|
|
This is the code for Arduino. I just tried to convert it into ccs c.
| Code: | #include <QTRSensors.h>
#define Kp 0 // experiment to determine this, start by something small that just makes your bot follow the line at a slow speed
#define Kd 0 // experiment to determine this, slowly increase the speeds and adjust this value. ( Note: Kp < Kd)
#define rightMaxSpeed 200 // max speed of the robot
#define leftMaxSpeed 200 // max speed of the robot
#define rightBaseSpeed 150 // this is the speed at which the motors should spin when the robot is perfectly on the line
#define leftBaseSpeed 150 // this is the speed at which the motors should spin when the robot is perfectly on the line
#define NUM_SENSORS 6 // number of sensors used
#define TIMEOUT 2500 // waits for 2500 us for sensor outputs to go low
#define EMITTER_PIN 2 // emitter is controlled by digital pin 2
#define rightMotor1 3
#define rightMotor2 4
#define rightMotorPWM 5
#define leftMotor1 12
#define leftMotor2 13
#define leftMotorPWM 11
#define motorPower 8
QTRSensorsRC qtrrc((unsigned char[]) { 14, 15, 16, 17, 18, 19} ,NUM_SENSORS, TIMEOUT, EMITTER_PIN); // sensor connected through analog pins A0 - A5 i.e. digital pins 14-19
unsigned int sensorValues[NUM_SENSORS];
void setup()
{
pinMode(rightMotor1, OUTPUT);
pinMode(rightMotor2, OUTPUT);
pinMode(rightMotorPWM, OUTPUT);
pinMode(leftMotor1, OUTPUT);
pinMode(leftMotor2, OUTPUT);
pinMode(leftMotorPWM, OUTPUT);
pinMode(motorPower, OUTPUT);
int i;
for (int i = 0; i < 100; i++) // calibrate for sometime by sliding the sensors across the line, or you may use auto-calibration instead
/* comment this part out for automatic calibration
if ( i < 25 || i >= 75 ) // turn to the left and right to expose the sensors to the brightest and darkest readings that may be encountered
turn_right();
else
turn_left(); */
qtrrc.calibrate();
delay(20);
wait();
delay(2000); // wait for 2s to position the bot before entering the main loop
/* comment out for serial printing
Serial.begin(9600);
for (int i = 0; i < NUM_SENSORS; i++)
{
Serial.print(qtrrc.calibratedMinimumOn[i]);
Serial.print(' ');
}
Serial.println();
for (int i = 0; i < NUM_SENSORS; i++)
{
Serial.print(qtrrc.calibratedMaximumOn[i]);
Serial.print(' ');
}
Serial.println();
Serial.println();
*/
}
int lastError = 0;
void loop()
{
unsigned int sensors[6];
int position = qtrrc.readLine(sensors); // get calibrated readings along with the line position, refer to the QTR Sensors Arduino Library for more details on line position.
int error = position - 2500;
int motorSpeed = Kp * error + Kd * (error - lastError);
lastError = error;
int rightMotorSpeed = rightBaseSpeed + motorSpeed;
int leftMotorSpeed = leftBaseSpeed - motorSpeed;
if (rightMotorSpeed > rightMaxSpeed ) rightMotorSpeed = rightMaxSpeed; // prevent the motor from going beyond max speed
if (leftMotorSpeed > leftMaxSpeed ) leftMotorSpeed = leftMaxSpeed; // prevent the motor from going beyond max speed
if (rightMotorSpeed < 0) rightMotorSpeed = 0; // keep the motor speed positive
if (leftMotorSpeed < 0) leftMotorSpeed = 0; // keep the motor speed positive
{
digitalWrite(motorPower, HIGH); // move forward with appropriate speeds
digitalWrite(rightMotor1, HIGH);
digitalWrite(rightMotor2, LOW);
analogWrite(rightMotorPWM, rightMotorSpeed);
digitalWrite(motorPower, HIGH);
digitalWrite(leftMotor1, HIGH);
digitalWrite(leftMotor2, LOW);
analogWrite(leftMotorPWM, leftMotorSpeed);
}
}
void wait(){
digitalWrite(motorPower, LOW);
} |
|
|
|
PCM programmer
Joined: 06 Sep 2003
Posts: 21708
|
|
| Posted: Thu Sep 07, 2017 6:50 pm |
|
|
Just google this:
| Quote: | | CCS compiler robotics exercise manual |
See chapter 10.
Then you already have the driver file if you have the CCS compiler.
The driver file is:
It's in your Drivers folder for the CCS compiler.
You don't have the CCS robot. You have your own robot.
Try and make it work.
This information should get you started. |
|
|
temtronic
Joined: 01 Jul 2010
Posts: 9229
Location: Greensville,Ontario
|
|
| Posted: Fri Sep 08, 2017 5:38 am |
|
|
This line:
| Quote: | | position = qtrrc.readLine(sensors); // get calibrated readings along with the line position, refer to the QTR Sensors Arduino Library for more details on line position. |
is the 'key' to your solution.
qtcc.readLine(sensors) appears to be a function that is in the 'library'. You'll have to locate it, convert to CCS C. THEN, your program may work. I don't have time to locate and examine.
I'm assuming the function is open sourced, if not, then you'll have to figure it out, perhaps as PCM P has suggested.
Jay |
|
|
srikrishna
Joined: 06 Sep 2017
Posts: 82
|
|
| Posted: Mon Nov 13, 2017 12:21 pm |
|
|
| Although i have written the code. But if there is any error please tell me. |
|
|
PCM programmer
Joined: 06 Sep 2003
Posts: 21708
|
|
| Posted: Mon Nov 13, 2017 4:29 pm |
|
|
In your translated code, you have this:
| Quote: |
while(1)
{
for (i=0;i<5;i++)
{
set_adc_channel(i);
sensor_value[i] = read_adc();
delay_us(15);
|
You are missing a delay after the set_adc_channel() line.
According to the 18F2550 data sheet, in this section, it says you must have
a minimum of 2.45 usec delay after changing the A/D channel:
| Quote: |
21.1 A/D Acquisition Requirements
TACQ = 2.45 μs
|
18F2550 data sheet:
http://ww1.microchip.com/downloads/en/DeviceDoc/39632e.pdf
I counted the instructions in the ASM code, and there is only about
1.6 usec between setting the A/D channel and setting the "Go" bit on
the A/D to start the conversion. So you're violating the spec.
I'm not sure why you have the delay_us(15) line in your program.
Maybe you intended it to be the "change A/D channel" delay and
it got put in the wrong place by mistake. But I suggest that you move it.
Put it here:
| Quote: |
set_adc_channel(i);
delay_us(15);
sensor_value[i] = read_adc();
|
Now you have the necessary delay. You can make it shorter if you want to. |
|
|
|
|
You cannot post new topics in this forum
You cannot reply to topics in this forum
You cannot edit your posts in this forum
You cannot delete your posts in this forum
You cannot vote in polls in this forum
|
Powered by phpBB © 2001, 2005 phpBB Group
|
FAQ 
Search
Register
Profile 
Log in to check your private messages 
Log in
