|
|
|
Module#7 : The software implementation The Software Algorithm used to drive the rover is described in the following sections. It involves the basic assumptions about the kind of environment where the rover will move due to the limitations in its capabilities. The Strategy section gives an idea of the kind of strategy being used in the development of the program. A brief explanation of the program logic is presented in the Algorithm Section. The Implementation Section briefs the basic mechanism used to move the rover. A SSUMPTIONS :The following assumptions hold true for the working of rover and form the basis for the development of the program. The assumptions also reflect the limitations of the rover and also demonstrate the capabitlities of the rover.
S TRATEGY :The Strategy followed dictates the logical structure of the program. The Strategy is based on the assumptions taken into consideration. The Strategy is enumerated in the following points : a. If the X co-ordinate difference is positive which means that the destination is to the right of the start. The Preferred Direction is "right". b. If the X co-ordinate difference is negative which means that the destination is to the left of the start. The Preferred Direction is "left". c. If no obstacle is encountered, then the rover moves straight along the Y distance and then turns depending on the Preferred Direction, moves to the right or to the left. d. If an obstacle is encountered, the following cases occur : i. If there is an obstacle in front and there are no obstacles on either side it turns 90 degrees and moves in the Preferred Direction. ii. If there is an obstacle in front AND there is an obstacle in the Preferred Direction then it 90 degrees and moves in the direction other than the Preferred Direction, but as soon as the obstacle is covered, it get back to the same vertical path as before. iii. If there is an obstacle in front AND there is an obstacle in the direction other than the Preferred Direction it 90 degrees and moves in th Preferred Direction and does not have to come back to the same vertical path as before, as it has covered the distance in the Preferred Direction. iv. If there is an obstacle in front and there are obstacles on either side, then it turns 180 degrees moves ahead thus effectively reversing the direction of the rover completely. THE ALGORITHM : The algorithm is as follows : 1. Initialise. 2. While destination is not reached Check status if the IR ports. if turn = left spin left and move ahead. else if turn = right spin right and move ahead. 3. Exit. THE IMPLEMENTATION :
The rover moved on wheels which were driven by motors and has shaft-encoders to count the distance travelled and control the number of revolutions. When a turn has to be taken one of the motors is stopped while the other moves thus making it turn in the required direction. Four digital inputs from the sensors provide the necessary inputs the program and and are quite instrumental in helping the program make decisions on its own.
T HE FINAL ALGORITHMmain() { check whether present at destination already; turn=check_IR(); while not at destination { spin(turn_angle); move(move_power); } we have reached destination;}int check_IR(){ if not moving in preferred direction(away from destination) { if right is preferred direction check IR sensor in that direction if no obstacle let us adjust turn_angle;return 1 else check IR sensors front of rover if obstacles detected check sensors to left if no obstacles present let us adjust turn_angle; return 1; else turn back; else move ahead;return 0; else (left is preferred direction) repeat above procedure for right is preferred direction } else (moving in preferred direction) { if no obstacles detected ahead move ahead;return 0; if obstacles are present in front of both sensors call check_IR() with not moving in preferred direction if there is obstacle in front of one of the sensors situated in the front check the sensor not adjacent to that sensor if obstacle is not detected adjust turn_angle in that direction return 1; else check sensor in diametrically opposite position if obstacle not present adjust turn_angle in other direction else retreat return 1; }}void spin(float turn_angle){ switch motors in opposite direction causing it to rotate about its central axis}void move(int move_power){ start both motors at 90% power in forward direction while (both motors are on) { Find which is preferred direction keep checking whether distance has been covered by at least one motor if so stop that motor adjust speed of the motor so that slipping is minimised check whether rover is moving north,south,east,west and adjust *x_rovptr or *y_rovptr; (note:if direction is north south y has to be changed else x) check whether x or y distance has been covered if so stop and rotate towards destination turn=check_IR() if (turn) get out of while loop } stop all motors}void retreat(void){ adjust the angle to PI find whether it is preferred direction or not}
T HE FINAL PROGRAM :
/* The constants for the program */ #define PI 3.1415926535898 #define X_DESTINATION 1000 #define Y_DESTINATION 1000 #define X_ROVER_START 0.0 #define Y_ROVER_START 0.0 #define COUNTS_PER_MM 0.150382623 #define VELOCITY_TOLERANCE 1 #define COUNTS_PER_RADIAN_CLK 19.54974099 #define COUNTS_PER_RADIAN_ANTICLK 19.54974099 #define TOLERANCE 250.0 #define WHEELBASE 260.0 #define X_LOWER_LIMIT 0.0 #define X_HIGHER_LIMIT 1048.0 #define Y_LOWER_LIMIT 0.0 #define Y_HIGHER_LIMIT 2248.0
/* Variable initialisation */ float *x_rovptr,*y_rovptr,x_dest,y_dest,turn_angle; int north=1,south,west,right=1,east,turn,turn_right,string_counter, check = 0; char send_string[100];
/* Function Declarations */ float Rovabs(float); int check_IR(void); int DestinationCheck(float,float,float,float); void spin(float); void move(int); void adjustdirection(int); int validate(void); void goback(); void goforward();
/* The String Copy Function */ void strcpy (char str1[], char str2[]) { int i = 0; while (str1[i]) { str2[i] = str1[i]; i++; } str2[i] = '\0'; }
/* The String to Integer Convertor Function */ int StringToInteger() { int i=0,ans,pos=1,j; while (send_string[i]!='\0') { int asc0=(int)'0'; for(j=0;j<9;j++) { if (asc0==(int)send_string[i]) { ans=ans+asc0*pos; break; } asc0++; } } return ans; } /* This function transmits a character through the serial line e.g. COM2*/ void serial_putchar(int c) { while (!(peek(0x102e) & 0x80)); /* wait until serial transmit empty */ poke(0x102f, c); /* send character */ } void disable_pcode_serial() /* necessary to receive characters using serial_g etchar */ { poke(0x3c, 1); } void reenable_pcode_serial() /* necessary for IC to interact with board again */ { poke(0x3c, 0); } int serial_getchar() { while (!(peek(0x102e) & 0x20)); /* wait for received character */ return peek(0x102f); } /* Functions used to transmit and receive data through the serial port */ void receive_data() { int data; disable_pcode_serial(); do { data=serial_getchar(); send_string[string_counter]=(char)data; string_counter++; } while (send_string[string_counter]!='\n'); send_string[string_counter]='\0'; reenable_pcode_serial(); } void transmit_data() { string_counter=-1; do { string_counter++; serial_putchar((int)send_string[string_counter]); } while (send_string[string_counter]!='\0'); } /* The main program */ main() { int move_power=90,at_end=0; *x_rovptr=X_ROVER_START; *y_rovptr=Y_ROVER_START; x_dest=X_DESTINATION; y_dest=Y_DESTINATION; printf ("\n Software Initialisation done.\n"); /*strcpy("\nWhat is the x coordinate? : ",send_string); transmit_data(); sleep(1); receive_data(); x_dest=StringToInteger(); strcpy("\nWhat is the y coordinate? : ",send_string); transmit_data(); sleep(1); receive_data(); y_dest=StringToInteger();*/ charge(); sleep(6); at_end = DestinationCheck(*x_rovptr,*y_rovptr,x_dest,y_dest); turn = check_IR(); while (!at_end) { if (4 & peek (0x1000)) check = !check; if ((turn)&&(check)) spin (turn_angle); if (check) move(move_power); at_end=DestinationCheck(*x_rovptr,*y_rovptr,x_dest,y_dest); } beep(); sleep(1); classics(); } /* Function for turning (i.e. spinning) the rover by é degrees */ void spin(float spin_angle) { int counts_target, velocity_delta; int spin_power = 90, spin_sign = 1, mot0 = 0, mot1 = 0,oldspin_power=90; float counts_per_radian = COUNTS_PER_RADIAN_CLK; reset_encoder (0); reset_encoder (1); if (spin_angle < 0.0) { spin_angle = -spin_angle; spin_sign=-1; counts_per_radian = COUNTS_PER_RADIAN_ANTICLK; } motor (0, (spin_sign*spin_power)); motor (1, (-1*spin_sign*spin_power)); counts_target = (int) (counts_per_radian*spin_angle); sleep(1); while( (mot0||mot1) == 0) { if (4 & peek (0x1000)) check = !check; if (!check) { ao(); do { sleep(1); if (4 & peek (0x1000)) check = !check; } while (!check); } motor(0,spin_sign*spin_power); motor(1,-1*spin_sign*oldspin_power); velocity_delta = (int) Rovabs ((float) encoder0_velocity-float)encoder1_velocity); if (((mot0 && mot1)==0) && (velocity_delta > VELOCITY_TOLERANCE) && (velocity_delta < 12)) { if (encoder1_velocity > encoder0_velocity) { if (spin_power<99) spin_power = spin_power + 2; } else { if (spin_power>40) spin_power = spin_power - 2; } motor (0, (spin_sign * spin_power)); } if (encoder0_counts >= counts_target) { off (0); mot0 = 1; } if (encoder1_counts >= counts_target) { off (1); mot1 = 1; } } ao(); } /* Function for deciding the direction of the rover */ void adjustdirection(int right) { int temp; if (right) { temp=east; east=north; north=west; west=south; south=temp; } else { temp=east; east=south; south=west; west=north; north=temp; } } void retreat(void) { turn_right=!turn_right; turn_angle=PI; adjustdirection(1); adjustdirection(1); } /* Function for checking the IR ports */ int check_IR(void) { int dig10,dig7,dig8,dig9; dig10=digital(10); dig7=digital(7); dig8=digital(8); dig9=digital(9); if (turn_right) { if (right) { if (!dig10) { turn_angle=PI/2.0; goback(); adjustdirection(1); return 1; } if (dig8||dig7) { if (!dig9) { turn_angle=-PI/2.0; goback(); adjustdirection(0); return 1; } else { retreat(); return 1; } } else { return 0; } } else if (!right) { if (!dig9) { turn_angle=-PI/2.0; goback(); adjustdirection(0); return 1; } else if (dig8||dig7) { if (!dig10) { turn_angle=PI/2.0; goback(); adjustdirection(1); return 1; } else { retreat(); return 1; } } else { return 0; } } } if (!turn_right) { if (!dig8&&!dig7) { return 0; } if (dig7 && dig8) { turn_right=1; turn=check_IR(); return turn; } if (dig7) if (!dig9) { turn_angle=-PI/2.0; goback(); adjustdirection(0); return 1; } else if (!dig10) { turn_angle=PI/2.0; goback(); adjustdirection(1); return 1; } else { retreat(); return 1; } if (dig8) if (!dig10) { turn_angle=PI/2.0; goback(); adjustdirection(0); return 1; } else if (!dig9) { turn_angle=-PI/2.0; goback(); adjustdirection(1); return 1; } else { retreat(); return 1; } } return 0; } /* Function for turning the rover back */ void goback(void) { int counts=23; bk(0); bk(1); do {} while (encoder1_counts <= counts); ao(); } /* Function for turning the rover forward */ /* void goforward(void) { int counts=23; fd(0); fd(1); do {} while (encoder1_counts <= counts); ao(); } */ /* Function to check whether the destination has been reached */ int DestinationCheck(float x_rov,float y_rov,float x_dest,float y_dest) { if ((Rovabs(x_dest-x_rov)<TOLERANCE)&&(Rovabs(y_dest-_rov)<TOLERANCE)) return 1; else return 0; } /* Function to move the rover by applying power to the motors */ void move(int move_power) { int mot0=0,mot1=0,sign,danger,old_power=move_power; float counts_distance,*coordinate,velocity_delta; if (north||south) { counts_distance=(int)(Rovabs(*y_rovptr-y_dest)*(float)COUNTS_PER_MM); if (*x_rovptr<x_dest) if (north) right=1;else right=0; else if (south) right=0;else right=1; } else { counts_distance=(int)(Rovabs(*x_rovptr-x_dest)*(float)COUNTS_PER_MM); if (*y_rovptr<y_dest) if (east) right=0;else right=1; else if (west) right=0;else right=1; } motor(0,move_power); motor(1,move_power); reset_encoder(0); reset_encoder(1); while (!(mot0||mot1)) { if (4 & peek (0x1000)) check = !check; if (!check) { ao(); do { sleep(1); if (4 & peek (0x1000)) check = !check; } while (!check); } motor(0,old_power); motor(1,move_power); if (*y_rovptr<y_dest) { if (north) turn_right=0; else if (south) turn_right=1; } else { if (south) turn_right=0; else if (north) turn_right=1; } if (*x_rovptr<x_dest) { if (east) turn_right=0; else if (west) turn_right=1; } else { if (west) turn_right=0; else if (east) turn_right=1; } if (read_encoder(0)>=counts_distance) { off(0); mot0=1; sleep(5); } if (read_encoder(1)>=counts_distance) { off(1); mot1=1; sleep(5); } if ((read_encoder(0)>32700)||(read_encoder(1)>32700)) { tone(1200.0,0.6); beep(); break; } velocity_delta=(int)(Rovabs((float)encoder0_velocity-(float)encoder1_velocity)); if ((mot0==0)&&(mot1==0)&&(velocity_delta > VELOCITY_TOLERANCE)&&(velocity_delta<12)) { if (encoder0_velocity > encoder1_velocity) { if (move_power<100) move_power+=2; } else { if (move_power>40) move_power-=2; } motor(1,move_power); } coordinate=x_rovptr; sign=1; if (west) { coordinate=x_rovptr; sign=-1; } else if (north) { coordinate=y_rovptr; sign=1; } else if (south) { coordinate=y_rovptr; sign=-1; } *coordinate+=((float)(sign))*(encoder1_counts/(float)COUNTS_PER_MM); danger=validate(); if (danger!=0) break; turn=check_IR(); if (turn) { ao(); mot0=mot1=1; break; } } ao(); } /* Returns the absolute value of an argument */ float Rovabs(float x) { if (x>=0.0) return (x); else return (-x); } /* Function to check the base ground limits for the rover */ int validate() { int a=0; if (((*x_rovptr>=X_HIGHER_LIMIT)&&east)||((*x_rovptr<X_LOWER_LIMIT)&&west) ||((*y_rovptr<Y_LOWER_LIMIT)&&south)||((*y_rovptr>=Y_HIGHER_LIMIT)&&north)) a=validateaction(); return a; } int validateaction() { ao(); turn_right=1; turn=check_IR(); if (!turn) { retreat(); spin(turn_angle); return 1; } else { spin(turn_angle); return 2; } } |
|
1999 IEEE VESIT Student Branch Hits :
Webmaster : Saumitra Mohan Das |
