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.
#include <REGX52.h> #include <math.h>unsigned char pwm_r,pwm_l,PWM;unsigned char stuck_counter;float line,max_speed;float error;float prop;float deriv;float integral;float correction;float previous_error=0;int mpos;float rate;unsigned char center=0;void delay5mS(void); // delay function ~ 5 mSvoid init_timer0(void);sbit Sensor_1 = P1^0; sbit Sensor_2 = P1^1;sbit Sensor_3 = P1^2;sbit Sensor_4 = P1^3;sbit Sensor_5 = P1^4;sbit Sensor_6 = P1^5;sbit sw = P3^7;sbit Motor1_2 = P3^0; // Motor 1_ Input1 sbit Motor1_3 = P3^1; // Motor 1_ Input2 sbit Motor2_2 = P2^0; // Motor 2_ Input1 sbit Motor2_3 = P2^1; // Motor 2_ Input2 #define KP 19 // proportionality coefficient#define KI 0.4 // Integral coefficient #define KD 0.4 // derviative coefficient void timer0(void) interrupt 1 using 0{ TR0=0; PWM++; TL0=0x9B; // ___ TF0=0; // | |_____ if (PWM==11) PWM=0; // if (PWM <= pwm_l) Motor1_2=0; else Motor1_2=1;; if (PWM <= pwm_r) Motor2_2=0; else Motor2_2=1; TR0=1; }void init_timer0(void) {EA=0; TMOD=0x02; // 0000 0010 TH0=0x9B; TL0=0x9B; PWM=0; ET0=1;EA=1; }fwd(){P2_0 = 1 ;P2_1 = 0 ;P3_0 = 1 ;P3_1 = 0 ;}stop(){P3_0 = 0 ;P3_1 = 0 ;P2_1 = 0 ;P2_0 = 0 ;}pivot_right(){P3_0 = 1 ;P3_1 = 0 ;P2_0 = 0 ;P2_1 = 1 ;}pivot_left(){P3_0 = 0 ;P3_1 = 1 ;P2_0 = 1 ;P2_1 = 0 ;}void main(){PWM= 11;pwm_r = 5;pwm_l = 5;max_speed =0;P0=0xFF;P1=0xFF;P2=0xFF;P3=0xFF;init_timer0(); while(1) { TR0=1;// start timer // Right Sidefwd();if (Sensor_1==0&&Sensor_2==0 && Sensor_3==0 &&Sensor_4==0 && Sensor_5==0&&Sensor_6==0){if (mpos<0)mpos=-5; else mpos=5;}else if (Sensor_1==0&&Sensor_2==0 && Sensor_3==0 &&Sensor_4==0 && Sensor_5==0&&Sensor_6==0){mpos=0;}else if (Sensor_1==0&&Sensor_2==0 &&Sensor_3==0&&Sensor_4==0 &&Sensor_5==0&&Sensor_6==1){mpos=6;}else if (Sensor_1==0&&Sensor_2==0&& Sensor_3==0&&Sensor_4==0&&Sensor_5==1&&Sensor_6==0){mpos=5;}else if (Sensor_1==0&&Sensor_2==0&& Sensor_3==0&&Sensor_4==0&&Sensor_5==1&&Sensor_6==1){mpos=5;}else if (Sensor_1==0&&Sensor_2==0 &&Sensor_3==0&&Sensor_4==1 &&Sensor_5==1&&Sensor_6==0){mpos=5;}else if (Sensor_1==0&&Sensor_2==0&& Sensor_3==1&&Sensor_4==1&&Sensor_5==1&&Sensor_6==0) {mpos=3;}else if (Sensor_1==0&&Sensor_2==0 &&Sensor_3==0&&Sensor_4==1 &&Sensor_5==0&&Sensor_6==0){mpos=2;}else if (Sensor_1==0&&Sensor_2==0 &&Sensor_3==0&&Sensor_4==1 &&Sensor_5==1&&Sensor_6==1){mpos=1;}// Left Sideelse if (Sensor_1==1&&Sensor_2==0 && Sensor_3==0&&Sensor_4==0&&Sensor_5==0&&Sensor_6==0){mpos=-6;}else if (Sensor_1==0&&Sensor_2==1 && Sensor_3==0&&Sensor_4==0&&Sensor_5==0&&Sensor_6==0){mpos=-5;}else if (Sensor_1==0&&Sensor_2==1 && Sensor_3==1&&Sensor_4==1&&Sensor_5==0&&Sensor_6==0){mpos=-4;}else if (Sensor_1==1&&Sensor_2==1 && Sensor_3==0&&Sensor_4==0&&Sensor_5==0&&Sensor_6==0){mpos=-4;}else if (Sensor_1==1&&Sensor_2==1 && Sensor_3==1&&Sensor_4==0&&Sensor_5==0&&Sensor_6==0){mpos=-3;}else if (Sensor_1==0&&Sensor_2==1 && Sensor_3==1&&Sensor_4==0&&Sensor_5==0&&Sensor_6==0){mpos=-2;}else if (Sensor_1==0&&Sensor_2==0 && Sensor_3==1&&Sensor_4==0&&Sensor_5==0&&Sensor_6==0){mpos=-1;}error=center-mpos;prop=error*KP;integral+=error;integral*=KI;rate=error - previous_error;deriv=rate*KD;previous_error=error;correction= prop+deriv+integral;if (correction>30) correction=30; // upper_limitif (correction<-30) correction=-30;// lower limitpwm_l=max_speed-correction;pwm_r=max_speed+correction;delay5mS(); // delay function ~ 5 mS} }
#include <REGX52.h> #include <math.h>int pwm_r,pwm_l,PWM;unsigned char stuck_counter;float line,max_speed;float error;float prop;float deriv;float integral;float correction;float previous_error=0;int mpos;float rate;unsigned char center=0;void delay5mS(void); // delay function ~ 5 mSvoid init_timer0(void);sbit Sensor_1 = P1^0; sbit Sensor_2 = P1^1;sbit Sensor_3 = P1^2;sbit Sensor_4 = P1^3;sbit Sensor_5 = P1^4;sbit Sensor_6 = P1^5;sbit sw = P3^7;sbit Motor1_2 = P3^0; // Motor 1_ Input1 sbit Motor1_3 = P3^1; // Motor 1_ Input2 sbit Motor2_2 = P2^0; // Motor 2_ Input1 sbit Motor2_3 = P2^1; // Motor 2_ Input2 #define KP 3 // proportionality coefficient#define KI 0.4 // Integral coefficient #define KD 0.4 // derviative coefficient void timer0(void) interrupt 1 using 0{ TR0=0; PWM++; TL0=0x9B; // ___ TF0=0; // | |_____ if (PWM==11) PWM=0; // if (PWM <= pwm_l) Motor1_2=0; else Motor1_2=1;; if (PWM <= pwm_r) Motor2_2=0; else Motor2_2=1; TR0=1; }void init_timer0(void) {EA=0; //Cam cac ngat hoat dong, xem thanh ghi IE TMOD=0x02; // 0000 0010 //Cai dat che do tu dong quay lai: che do 2TH0=0x9B; //Chu ki cai dat thong qua VDK la 1 chu ky may : 1.085usTL0=0x9B; PWM=0; ET0=1; //Cho phep ngat do Timer0 //Khoi dong Timer0EA=1; //Cho phep tat ca cac ngat hoat dong }fwd(){P2_0 = 1 ;P2_1 = 0 ;P3_0 = 1 ;P3_1 = 0 ;}stop(){P3_0 = 0 ;P3_1 = 0 ;P2_1 = 0 ;P2_0 = 0 ;}pivot_right(){P3_0 = 1 ;P3_1 = 0 ;P2_0 = 0 ;P2_1 = 1 ;}pivot_left(){P3_0 = 0 ;P3_1 = 1 ;P2_0 = 1 ;P2_1 = 0 ;}void main(){PWM= 11;max_speed =0; P0=0xFF;P1=0xFF;P2=0xFF;P3=0xFF;init_timer0(); while(1) { TR0=1;// start timer // Right Side fwd();if (Sensor_1==0&&Sensor_2==0 && Sensor_3==0 &&Sensor_4==0 && Sensor_5==0&&Sensor_6==0){if (mpos<0)mpos=-7; else mpos=7;}else if (Sensor_1==1&&Sensor_2==1 && Sensor_3==1 &&Sensor_4==1 && Sensor_5==1&&Sensor_6==1){mpos=0;}else if (Sensor_1==0&&Sensor_2==0 &&Sensor_3==0&&Sensor_4==0 &&Sensor_5==0&&Sensor_6==1){mpos=6;}else if (Sensor_1==0&&Sensor_2==0&& Sensor_3==0&&Sensor_4==0&&Sensor_5==1&&Sensor_6==0){mpos=5;}else if (Sensor_1==0&&Sensor_2==0&& Sensor_3==0&&Sensor_4==0&&Sensor_5==1&&Sensor_6==1){mpos=4;}else if (Sensor_1==0&&Sensor_2==0 &&Sensor_3==0&&Sensor_4==1 &&Sensor_5==1&&Sensor_6==0){mpos=4;}else if (Sensor_1==0&&Sensor_2==0&& Sensor_3==1&&Sensor_4==1&&Sensor_5==1&&Sensor_6==0) {mpos=3;}else if (Sensor_1==0&&Sensor_2==0 &&Sensor_3==0&&Sensor_4==1 &&Sensor_5==0&&Sensor_6==0){mpos=2;}else if (Sensor_1==0&&Sensor_2==0 &&Sensor_3==0&&Sensor_4==1 &&Sensor_5==1&&Sensor_6==1){mpos=1;}// Left Sideelse if (Sensor_1==1&&Sensor_2==0 && Sensor_3==0&&Sensor_4==0&&Sensor_5==0&&Sensor_6==0){mpos=-6;}else if (Sensor_1==0&&Sensor_2==1 && Sensor_3==0&&Sensor_4==0&&Sensor_5==0&&Sensor_6==0){mpos=-5;}else if (Sensor_1==1&&Sensor_2==1 && Sensor_3==0&&Sensor_4==0&&Sensor_5==0&&Sensor_6==0){mpos=-4;}else if (Sensor_1==0&&Sensor_2==1 && Sensor_3==1&&Sensor_4==0&&Sensor_5==0&&Sensor_6==0){mpos=-4;}else if (Sensor_1==1&&Sensor_2==1 && Sensor_3==1&&Sensor_4==0&&Sensor_5==0&&Sensor_6==0){mpos=-3;}else if (Sensor_1==0&&Sensor_2==0 && Sensor_3==1&&Sensor_4==0&&Sensor_5==0&&Sensor_6==0){mpos=-2;}else if (Sensor_1==0&&Sensor_2==1 && Sensor_3==1&&Sensor_4==1&&Sensor_5==0&&Sensor_6==0){mpos=-1;} error=center-mpos; prop=error*KP; integral+=error; integral*=KI; rate=error - previous_error; deriv=rate*KD; previous_error=error;correction = (prop-deriv);if (correction >10.0)correction=10.0;if (correction <-30.0)correction=0.0;pwm_l=max_speed+correction;pwm_r=max_speed-(correction);delay5mS(); // delay function ~ 5 mS } }
and about the member robot tutorial , it will also never be able to take 90degree turns coz the sensors are all arranged linearly thats the problem with my bot too as i have used just 3 sensors
how did u make it do 90 degree it can it be done with 3 sensors
what u have done was already thought earlier by me ,it has a problems as the first 2 will always come out first and the bot wouldnt be able to know where to turn ,also the ending point will not be recognised by the bot
ihavent yet been able to find out a solution using 3 sensors so if u could help then it would be cool
adjust motor speeds by adjusting PWM for each individual motor or by adding a resistor to the motor.
also when the robot is positioned like figure 2 and make it to follow the line it doesnt ... it just cross it as it and doesnt follow it !!