void soft_left_degrees(unsigned int Degrees)
{
// 176 pulses for 360 degrees rotation 2.045 degrees per count
soft_left(); //Turn soft left
Degrees=Degrees*2;
angle_rotate(Degrees);
}
void left_degrees(unsigned int Degrees)
{
// 28 pulses for 360 degrees rotation 12.92 degrees per count
//left(); //Turn left
//angle_rotate(Degrees);
soft_left(); //Turn soft left
Degrees=Degrees*2;
angle_rotate(Degrees);
}
//Main Function
int main()
{
init_devices();
velocity (100, 100); //Set robot velocity here. Smaller the value lesser will be the velocity
//Try different valuse between 0 to 255
while(1)
{
forward(); //both wheels forward
_delay_ms(1000);
stop();
_delay_ms(500);
back(); //both wheels backward
_delay_ms(1000);
stop();
_delay_ms(500);
left(); //Left wheel backward, Right wheel forward
_delay_ms(1000);
stop();
_delay_ms(500);
right(); //Left wheel forward, Right wheel backward
_delay_ms(1000);
stop();
_delay_ms(500);
soft_left(); //Left wheel stationary, Right wheel forward
_delay_ms(1000);
stop();
_delay_ms(500);
soft_right(); //Left wheel forward, Right wheel is stationary
_delay_ms(1000);
stop();
_delay_ms(500);
soft_left_2(); //Left wheel backward, right wheel stationary
_delay_ms(1000);
stop();
_delay_ms(500);
soft_right_2(); //Left wheel stationary, Right wheel backward
_delay_ms(1000);
stop();
_delay_ms(1000);
}
}
//Main Function
int main()
{
init_devices();
while(1)
{
forward(); //both wheels forward
_delay_ms(1000);
stop();
_delay_ms(500);
back(); //bpth wheels backward
_delay_ms(1000);
stop();
_delay_ms(500);
left(); //Left wheel backward, Right wheel forward
_delay_ms(1000);
stop();
_delay_ms(500);
right(); //Left wheel forward, Right wheel backward
_delay_ms(1000);
stop();
_delay_ms(500);
soft_left(); //Left wheel stationary, Right wheel forward
_delay_ms(1000);
stop();
_delay_ms(500);
soft_right(); //Left wheel forward, Right wheel is stationary
_delay_ms(1000);
stop();
_delay_ms(500);
soft_left_2(); //Left wheel backward, right wheel stationary
_delay_ms(1000);
stop();
_delay_ms(500);
soft_right_2(); //Left wheel stationary, Right wheel backward
_delay_ms(1000);
stop();
_delay_ms(1000);
}
}
void my_velocity(float val,float scale, int offset, float drive)
{
unsigned char velocity = scale*drive + offset;
if(val>0)
{
if(val<scale+offset)
soft_right(velocity,0);
else
right(velocity,-velocity);
}
else
{
if(val>-(scale+offset))
soft_left(0,velocity);
else
left(-velocity,velocity);
}
}
void move()
{
int i;
/*variable to use in figuring out the "best" option*/
int max_q_score = 0;
/*what do we do next? store it here*/
/*we init to -1 as an error*/
int next_movement = -1;
/*Where we started.*/
/*We don't use ROTATION_2 all the way through in case it changes.*/
int initial_angle = norm_rotation(ROTATION_2);
/*Where we ended up.*/
int new_angle;
/*Show the current angle*/
cputc_native_user(CHAR_A, CHAR_N, CHAR_G, CHAR_L); // ANGL
msleep(200);
lcd_int(initial_angle);
msleep(500);
/*
* Most of the time, we do the "correct" thing
* by finding the best q_score of our possible options.
* On the off chance that norm_random() is low (or EPSILON is high ;)
* we then "explore" by choosing a random movement.
*/
if(norm_random() > EPSILON_CURRENT)
{
/*We are doing what the table tells us to.*/
cputc_native_user(CHAR_r, CHAR_e, CHAR_a, CHAR_l); // real
msleep(500);
for(i=0; i<MOVEMENTS; i++)
{
if(q_score[initial_angle][i] > max_q_score)
{
max_q_score = q_score[initial_angle][i];
next_movement = i;
}
}
}
else
{
double temp;
/*We are just picking something at random.*/
cputc_native_user(CHAR_r, CHAR_a, CHAR_n, CHAR_d); // rand
msleep(500);
/*pick one. Any one.*/
temp = norm_random();
next_movement = temp*MOVEMENTS;
/*show what we do next*/
lcd_int(next_movement);
sleep(1);
}
/*what happens if next_movement never gets changed?*/
/*we'd hate to do HARD_LEFT over and over again*/
/*so we choose randomly*/
if(-1==next_movement)
{
double temp;
temp = norm_random();
next_movement = temp*MOVEMENTS;
}
/*having chosen a movement, lets do it*/
switch(next_movement)
{
case HARD_LEFT:
cputc_native_user(CHAR_H, CHAR_L, 0, 0); // HL
hard_left();
break;
case SOFT_LEFT:
cputc_native_user(CHAR_S, CHAR_L, 0, 0); // SL
soft_left();
break;
case FORWARD:
cputc_native_user(CHAR_F, CHAR_W, CHAR_W, CHAR_D); // FWD
go_forward();
break;
case SOFT_RIGHT:
cputc_native_user(CHAR_S, CHAR_R, 0, 0); // SR
soft_right();
break;
case HARD_RIGHT:
cputc_native_user(CHAR_H, CHAR_R, 0, 0); // HR
hard_right();
break;
case REVERSE:
cputc_native_user(CHAR_R, CHAR_E, CHAR_V, 0); // REV
go_back();
break;
default:
/*this is an error and should never be reached*/
cputc_native_user(CHAR_E, CHAR_R, CHAR_R, 0); // ERR
stop_motors();
sleep(1);
break;
}
/*Once we've started, we'd better stop*/
stop_motors();
/*Allows us to read direction*/
msleep(500);
/*This is here just to make the next function cleaner*/
new_angle = norm_rotation(ROTATION_2);
/*Where are we now?*/
cputc_native_user(CHAR_N, CHAR_E, CHAR_W, CHAR_W); // NEW
msleep(200);
lcd_int(new_angle);
msleep(500);
/*
* Since we know that "next_movement" took us from "initial_angle"
* to new_angle (ROTATION_2), we store that increased probability.
*/
steering_results[initial_angle][next_movement][new_angle] += ALPHA;
/*here we re-norm so that the sum of the probabilities is still 1*/
for(i=0; i<ANGLES; i++)
{
steering_results[initial_angle][next_movement][i] /= (1+ALPHA);
}
/*The last thing we do is reduce Epsilon*/
if(EPSILON_CURRENT > EPSILON_MIN)
{
EPSILON_CURRENT-=EPSILON_DECAY;
}
}
//Main Function
int main(void)
{
unsigned char flag ;
init_devices();
lcd_set_4bit();
lcd_init();
velocity(VELOCITY_MAX,VELOCITY_MAX); // Set the speed to max velocity
forward(); // start to move froward
unsigned char lch=0;
unsigned char rch=0;
unsigned char cch=0;
int ler=0;
int rer=0;
float thd=10;
int per=0;
int thdh=80;
int thdl=20;
float kp=10;
int kd=50;
int rl=0,old_rl=0;
int PWM_ratio_1 = 10;
int PWM_ratio_2 = 25;
unsigned char old_ler=0;
unsigned char old_rer=0;
unsigned char old_per=100;
while(1)
{
lch=ADC_Conversion(3);
rch=ADC_Conversion(5);
cch=ADC_Conversion(4);
ler=lch-cch;
rer=rch-cch;
per=lch-rch;
rl=rch-lch;
print_sensor(1,1,3); //Prints value of White Line Sensor Left
print_sensor(1,5,4); //Prints value of White Line Sensor Center
print_sensor(1,9,5); //Prints value of White Line Sensor Right
if(rl>0)
{
thd = 120 - (kp*rl+kd*(rl-old_rl));
}
else
{
thd = -120 - (kp*rl+kd*(rl-old_rl));
}
lcd_print (2,1,abs(rl),3);
lcd_print (2,5,(unsigned char)abs(thd),3);
if(rl>(-thd) && rl<(thd))
{
forward();
//velocity(255,255);
}
else
{
if(rl>0)
{
if(rl<thd+30)
soft_right();
//velocity(min(255,rl*PWM_ratio_1),0);
else
right();
//velocity(min(255,rl*PWM_ratio_2),0);
}
else
{
if(rl>-thd-30)
soft_left();
//velocity(0,min(255,-rl*PWM_ratio_1));
else
left();
//velocity(0,min(255,-rl*PWM_ratio_2));
}
}
old_rl=rl;
}
}
