Don't ad-block us - support your favorite websites. We have safe, unobstrusive, robotics related ads that you actually want to see - see here for more.
0 Members and 1 Guest are viewing this topic.
[color=red][font=Verdana]#include <stdio.h>#include <P18CXXX.h>#include <adc.h>#include <timers.h>#pragma code page // This code make sure the program starts from 0x0004, not from 0x0000int lasttrack =0; // variable used to decide which side the motor should rotate if it escapes the trackint trackline(void);int findline(void);void OpenTimer2( unsigned char config );void OpenPWM1( char period );void OpenPWM2( char period );void SetDCPWM1( unsigned int dutycycle );void SetDCPWM2( unsigned int dutycycle );void main(){ int trackstatus; OpenPWM1(0xff); // init PWM1 OpenPWM2(0xff); // init PWM1 OpenTimer2 ( TIMER_INT_OFF & T2_PS_1_16 & T2_POST_1_8 ); // init Timer2 TRISB = 0b00011111; // Port,pin direction configuration PORTB = 0; TRISC = 0; PORTC = 0; TRISCbits.TRISC7 = 1; // make sure this pin is input while (1) { trackstatus = trackline(); }}// The middle 5 sensors of the tracker are connected from PORTB0 to PORTB4. PORTB0 is connected to the right most and PORTB4 is connected to the left most// Right Motor is motor1, controlled by RC0 (m1d1), RC3 (m2d2) and PWM1 (RC1)// Left Motor is motor2, controlled by RC4 (m2d1), RC5 (m2d2) and PWM2 (RC2)int trackline(){ int track, trackin; int basespeed=600, offset=0, offb=60; while (1) { trackin = 0b11111111 ^ PORTB; // The inputs from the line sensor are high on white and Low on Black. I'm tracking a black line so the sensors go low when I am on the line. // The inputs are inverted to a variable 'trackin' so that it makes thinking easier (1- on the line, 0 - off the line) track = trackin & 31; //variable 'track' will indicate the status of the line sensors alone - The other 3 pins of the digital input port (PORTB) are to be used for other stuff // So basically the last 5 bits of the variable 'track' will indicate the status of the line sensors. // The right most bit corresponds to the outer most sensor on the right, The 5th bit corresponds to the outer most on the left // and the 3rd bit is the sensor in the centre in the array of line sensors switch (track) { case 0b00000100: case 0b00001110: basespeed = 600; offset = 0; break; // Line is in the dead centre of the tracker => being followed properly case 0b00000001: basespeed = 550; offset = 5; break; case 0b00000011: basespeed = 550; offset = 4; break; case 0b00000010: basespeed = 550; offset = 3; break; case 0b00000110: basespeed = 550; offset = 1; break; /////////////////////////////////////////////////////////////////////// case 0b00010000: basespeed = 550; offset = -5; break; case 0b00011000: basespeed = 550; offset = -4; break; case 0b00001000: basespeed = 550; offset = -3; break; case 0b00001100: basespeed = 550; offset = -1; break; default: basespeed = 600; offset = 0; }// *********Drive motors according to inferences from variable 'track'********************** PORTCbits.RC0 = 1; // Rotate right motor forward PORTCbits.RC3 = 0; PORTCbits.RC5 = 0; // Rotate left motor forward PORTCbits.RC4 = 1; SetDCPWM2(basespeed + offset*offb); // Set duty cycle value for right motor ; offb is set earlier in the code, offb=60 SetDCPWM1(basespeed - offset*offb); // Set duty cycle value for left motor // When the argument for SetDCPWM1() is 1023 then it is 100 % duty//-********************************************************************************* }}[/font][/color]
The swinging effect was less (surprisingly) but the robot seemed to jerk a lot - so I thought I'd drop it.
4. I'm not planning on using encoders for the moment. (Correct me if I'm wrong here - but with encoders you can judge the speed of the motors, which you can use to implement PD and that can reduce the oscillating effect). So if there's a way I can softcode PD (ie try and predict the velocity and hence implement PD) please let me know.
He only has six sensors to detect the line. The code uses a hardware timer (TMR0 in a PIC) and gets the sensor measurements every timer tick at a 9600Hz rate. Then sorts through the sensors measurements to determine how far off of the line center the robot is and how fast it got there (this is the reason for a timer). These are then applied the the PWM duty cycle to speed up or slow down the right or left motor to keep the robot on the line.
By using a timer you will have the amount of error in a fixed time interval to apply to the PWM (proportional term).
#include <stdio.h>#include <P18CXXX.h>#include <adc.h>#include <timers.h>#pragma code page // This code make sure the program starts from 0x0004, not from 0x0000// GLOBAL VARIABLESint t0_int_count = 0, t1_int_count =0;int timer1_start =0, timer0_start = 0, timer3_start =61000;char mode = 'f';char track=0, trackin = 255, lasttrack=0, lasttrack_count = 0; // Variables that will maintain the status of the line following sensors and aid in tracking the line;char trackstatus = 0;char esctrack =0; // variable used to decide which side the motor should rotate if it escapes the trackint offb=0, last_offb=0, pd_basespeed = 10000; // Variables controlling the derivative term/////////////////////////////////////////// FUNCTION PROTOTYPES//////////////////////////////////////////void OpenTimer2( unsigned char config );void OpenPWM1( char period );void OpenPWM2( char period );void SetDCPWM1( unsigned int dutycycle );void SetDCPWM2( unsigned int dutycycle );void OpenADC( unsigned char config,unsigned char config2 );void ConvertADC( void );void WriteTimer0( unsigned int timer );void WriteTimer1( unsigned int timer );void WriteTimer3( unsigned int timer );///////////////////////////////////////////void timer0init_enc(void); void timer0_isr(void);void timer1init_enc(void); void timer1_isr(void);void timer3init_line(void); void timer3_isr(void); void timer3tmnt_line(void);void modules_initialize(void);void ports_initialize(void);void startmotorfwd(int max);int followline(void);void findline(void);void trackline(void);/////////////////////////////////////////void high_isr(void);////////////////////////////////////////void main(void){ ports_initialize(); modules_initialize(); followline(); // Follow the line while (1) {}}//--------------------------------------------------------------------// Misc Functionsvoid startmotorfwd(int max) // to start the motor without any jerk{ int i,n; PORTCbits.RC0 = 1; // Start right motor PORTCbits.RC3 = 0; PORTCbits.RC5 = 0; // Start left motor PORTCbits.RC4 = 1; for (i=0; i<max; i++) { SetDCPWM2(i); // Set duty cycle value for PWM2 SetDCPWM1(i); // Set duty cycle value for PWM1 for (n=0; n<=1500; n++) // Create a delay so that the motor has enough time to start. { _asm nop _endasm } }}int followline(void) // will start up and initialize timer3 in order to track the line{ trackin = 255 ^ PORTB; track = 31 & trackin; // Now track reads the current position of the line follower // if (track == 0) findline(); // First find the line if you are not on the track timer3init_line(); trackstatus = 0; lasttrack_count = 0; PORTCbits.RC0 = 1; PORTCbits.RC3 = 0; // Rotate both motors forward PORTCbits.RC4 = 1; PORTCbits.RC5 = 0; SetDCPWM1(400); // Initial speed to start up motors SetDCPWM2(400); while (!trackstatus) { trackin = 255 ^ PORTB; track = 31 & trackin; // Now track reads the current position of the line follower esctrack = track; // Escape conditions: If any of the following conditions are encountered stop following the track; if(PORTBbits.RB5 == 0 & track != 0) trackstatus = 'r'; if(PORTBbits.RB6 == 0 & track != 0) trackstatus = 'l'; if(track == 0) trackstatus = 'n'; } timer3tmnt_line();}void trackline(void){ int rightspeed=0, leftspeed=0; // Variables to assign values to the two speeds int rbs, lbs, offset = 100; // Variables controlling the proportional term switch (track) { case 0b00000100: case 0b00001110: lbs = 4*offset; rbs = 4*offset; break; // Line is in the dead centre of the tracker => being followed properly case 0b00000001: lbs = 4*offset; rbs = 0; break; case 0b00000011: lbs = 4*offset; rbs = offset; break; case 0b00000111: case 0b00000010: lbs = 4*offset; rbs = 2*offset; break; case 0b00000110: lbs = 4*offset; rbs = 3*offset; break; /////////////////////////////////////////////////////////////////////// case 0b00010000: rbs = 4*offset; lbs = 0; break; case 0b00011000: rbs = 4*offset; lbs = offset; break; case 0b00011100: case 0b00001000: rbs = 4*offset; lbs = 2*offset; break; case 0b00001100: rbs = 4*offset; lbs = 3*offset; break; default: rbs = 100; lbs = 100; } if (rbs < lbs) offb = 4 - rbs/offset; else if (lbs < rbs) offb = -(4 - lbs/offset); lasttrack_count ++; /* eg: If track = 0b00011000 at one point and after 250 interrupts of timer3 track = 0b00001000 offb = -(3-2) = -1; lasttrack_count = 250; rbs = 400; lbs = 200; rightspeed = 400 + (-1)*10000/250 = 360; // See below leftspeed = 200 - (-1)*10000/250 = 240; Similarly had the same transition occurred at a slower speed such that it took 400 interrupts of timer3 rightspeed = 400 + (-1)*10000/400 = 380; // See below leftspeed = 200 - (-1)*10000/400 = 220; */ if (last_offb != offb) // Execute this loop iff the status of the line tracker has changed { rightspeed = rbs + (last_offb - offb)*pd_basespeed/lasttrack_count; leftspeed = lbs - (last_offb - offb)*pd_basespeed/lasttrack_count; SetDCPWM1(leftspeed); SetDCPWM2(rightspeed); lasttrack_count=0; } lasttrack = track; last_offb = offb;}// Misc Functions .......//---------------------------------------------------------------------//--------------------------------------------------------------------// Functions to set up modules for operationvoid timer0init_enc(void) // initialize timer0 to receive inputs from the encoder in the right wheel{ TRISAbits.TRISA4 = 1; // Ensure that RA4/T0CKI is an input //INTCON2bits.TMR0IP = 1; // High interrupt priority - not necessary if OpenTimer0( TIMER_INT_ON & T0_16BIT & T0_SOURCE_EXT & T0_PS_1_1 ); t0_int_count = 0;}void timer1init_enc(void) // initialize timer1 to receive inputs from the encoder in the left wheel{ TRISCbits.TRISC0 = 1; //IPR1bits.TMR1IP = 1; OpenTimer0( TIMER_INT_ON & T1_16BIT_RW & T1_SOURCE_EXT & T0_PS_1_1 ); t1_int_count = 0;}void timer3init_line(void){ OpenTimer3( TIMER_INT_ON & T3_16BIT_RW & T3_SOURCE_INT & T3_PS_1_1 ); WriteTimer3(timer3_start);}void timer3tmnt_line(void){ CloseTimer3();}// Functions to set up modules for operation.......//--------------------------------------------------------------------//------------------------------------------------------------------// Startup initialization functionsvoid modules_initialize(void){ OpenPWM1(0xff); // initialize PWM OpenPWM2(0xff); // OpenTimer2 ( TIMER_INT_OFF & T2_PS_1_16 & T2_POST_1_8 ); // OpenADC (ADC_FOSC_32 & ADC_RIGHT_JUST & ADC_5ANA_0REF, ADC_CH0 & ADC_INT_OFF ); // initialize ADC RCONbits.IPEN = 0; // Disable interrupt priority // initialize Timer Interrupts INTCONbits.GIE = 1; // enable Global interrupts for the moment INTCONbits.PEIE = 1; // enable Peripheral interrupts for the moment}void ports_initialize(void){ TRISB = 255; PORTB = 0; TRISC = 0; PORTC = 0; TRISCbits.TRISC7 = 1; // make sure this pin is an input TRISA = 0b00011111; PORTA = 0; TRISD = 0b11111100; PORTD = 0;}// Startup initialization functions....//-------------------------------------------------------------------//---------------------------------------------------------------------// Functions handling interrupts#pragma code high_vector=0x08void interrupt_at_high_vector(void){ _asm GOTO high_isr _endasm}#pragma code /* return to the default code section */#pragma interrupt high_isrvoid high_isr (void){ if (INTCONbits.TMR0IF == 1) timer0_isr(); // timer0 interrupt if (PIR1bits.TMR1IF == 1) timer1_isr(); // timer1 interrupt if (PIR2bits.TMR3IF == 1) timer3_isr(); // timer3 interrupt}// Functions handling interrupts........//----------------------------------------------------------------------//-----------------------------------------------------------------------// ISR'svoid timer0_isr(void){ WriteTimer0(timer1_start); INTCONbits.TMR0IF = 0; // Clear flag before RETFIE}void timer1_isr(void){ WriteTimer1(timer0_start); PIR1bits.TMR1IF = 0; // Clear flag before RETFIE}void timer3_isr(void){ trackline(); WriteTimer3(timer3_start); PIR2bits.TMR3IF = 0;}// ISR's .......// -----------------------------------------------------------------------
@ Admin:How do you use fuzzy logic. Is it something I can learn and implement within a couple of days?