Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- #pragma config(Sensor, S2, gyroSensor, sensorEV3_Gyro, modeEV3Gyro_RateAndAngle)
- #pragma config(Sensor, S3, Colourright, sensorEV3_Color, modeEV3Color_Reflected)
- #pragma config(Sensor, S1, Colourleft, sensorEV3_Color, modeEV3Color_Reflected)
- #pragma config(Sensor, S4, sonarSensor, sensorEV3_Ultrasonic)
- #pragma config(Motor, motorA, armMotor, tmotorEV3_Medium, PIDControl, driveLeft, encoder)
- #pragma config(Motor, motorB, leftMotor, tmotorEV3_Large, PIDControl, driveLeft, encoder)
- #pragma config(Motor, motorC, rightMotor, tmotorEV3_Large, PIDControl, driveRight, encoder)
- //*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//
- int check_map[9][9],map_up[9][9],map_right[9][9],map_left[9][9],map_down[9][9];
- int available_block=0,n=9,short_map[9][9],position_temp[90][2],chk_finish = 0;
- int x=23;
- void checkSoundFile(const char *pFileName)
- {
- bool bExists;
- bExists = bEv3FileExists(pFileName);
- while (!bSoundQueueAvailable)
- {}
- playSoundFile(pFileName);
- return;
- }
- void checklineback(){
- clearTimer(T1);
- checkSoundFile("Backwards");
- wait1Msec(500);
- checkSoundFile("Backing alert");
- while (true)
- {
- // Write the amount of reflected light to the screen
- // This is a value between 0 and 100, where 0 means no reflected
- // light and 100 means all light is being reflected
- displayTextLine(14," checkLineBack = %d",time1[T1]);
- if(time1[T1] < 8000){
- if(SensorValue[S1]<x&&SensorValue[S3]>x){
- motor[motorB]=-2;
- motor[motorC]=0;
- }else if(SensorValue[S1]>x&&SensorValue[S3]<x){
- motor[motorB]=0;
- motor[motorC]=-2;
- }else if(SensorValue[S1]<x&&SensorValue[S3]<x){
- motor[motorB]=-15;
- motor[motorC]=-15;
- }else if((SensorValue[S1]>x&&SensorValue[S3]>x)||(((SensorValue[S1]-SensorValue[S3])>-2)&&((SensorValue[S1]-SensorValue[S3])<2))){
- break;
- }
- }
- else break;
- // Wait 20 ms to get 50 readings per second
- sleep(20);
- }
- motor[motorB] = 0;
- motor[motorC] = 0;
- wait1Msec(500);
- }
- void walkbackfromline(){
- clearTimer(T1);
- while(true){
- displayTextLine(14,"walkBackFromLine = %d",time1[T1]);
- if(time1[T1] < 15000){
- if(SensorValue[S1]>x-3&&SensorValue[S3]>x-3){
- motor[motorB]=-1;
- motor[motorC]=-1;
- }else if(SensorValue[S1]<x-6&&SensorValue[S3]>x-6){
- motor[motorB]=0;
- motor[motorC]=-1;
- }else if(SensorValue[S1]>x-6&&SensorValue[S3]<x-6){
- motor[motorB]=-1;
- motor[motorC]=0;
- }else if(SensorValue[S1]<x-6&&SensorValue[S3]<x-6){
- break;
- }
- }
- else break;
- sleep(20);
- }
- motor[motorB] = 0;
- motor[motorC] = 0;
- wait1Msec(500);
- checkSoundFile("Backwards");
- wait1Msec(500);
- checkSoundFile("Backing alert");
- moveMotorTarget(motorB,130,-30);
- moveMotorTarget(motorC,130,-30);
- wait1Msec(500);
- }
- void checkline(){
- clearTimer(T1);
- checkSoundFile("Forward");
- wait1Msec(500);
- checkSoundFile("Flashing");
- while (true)
- {
- // Write the amount of reflected light to the screen
- // This is a value between 0 and 100, where 0 means no reflected
- // light and 100 means all light is being reflected
- displayTextLine(14,"checkLine = %d",time1[T1]);
- if(time1[T1] < 5000){
- if(SensorValue[S1]<x&&SensorValue[S3]>x){
- motor[motorB]=2;
- motor[motorC]=-2;
- }else if(SensorValue[S1]>x&&SensorValue[S3]<x){
- motor[motorB]=-2;
- motor[motorC]=2;
- }else if(SensorValue[S1]<x&&SensorValue[S3]<x){
- motor[motorB]=15;
- motor[motorC]=15;
- }else if((SensorValue[S1]>x&&SensorValue[S3]>x)||(((SensorValue[S1]-SensorValue[S3])>-2)&&((SensorValue[S1]-SensorValue[S3])<2))){
- break;
- }
- }
- else break;
- // Wait 20 ms to get 50 readings per second
- sleep(20);
- }
- motor[motorB]=0;
- motor[motorC]=0;
- wait1Msec(500);
- }
- void walkfromline(){
- clearTimer(T1);
- while(true){
- displayTextLine(14,"walkFromLine = %d",time1[T1]);
- if(time1[T1] < 5000){
- if(SensorValue[S1]>x&&SensorValue[S3]>x){
- motor[motorB]=2;
- motor[motorC]=2;
- }else if(SensorValue[S1]<x-6&&SensorValue[S3]>x-6){
- motor[motorB]=0;
- motor[motorC]=2;
- }else if(SensorValue[S1]>x-6&&SensorValue[S3]<x-6){
- motor[motorB]=2;
- motor[motorC]=0;
- }else if(SensorValue[S1]<x-6&&SensorValue[S3]<x-6){
- break;
- }
- }
- else break;
- sleep(20);
- }
- motor[motorB] = 0;
- motor[motorC] = 0;
- wait1Msec(500);
- checkSoundFile("Forward");
- wait1Msec(500);
- checkSoundFile("Flashing");
- moveMotorTarget(motorB,400,30);
- moveMotorTarget(motorC,400,30);
- wait1Msec(500);
- }
- int check_right(){
- resetMotorEncoder(motorA);
- while(getMotorEncoder(motorA) < 110){
- setMotorSpeed(motorA,10);
- }
- setMotorSpeed(motorA,0);
- int a = getUSDistance(S4);
- return a;
- }
- int check_left(){
- resetMotorEncoder(motorA);
- while(getMotorEncoder(motorA) > -110){
- setMotorSpeed(motorA,-10);
- }
- setMotorSpeed(motorA,0);
- int b = getUSDistance(S4);
- return b;
- }
- int check_positionY(int direction,int x,int y){
- if(direction == 1){
- return y;
- }
- else if(direction == 2){
- return y+1;
- }
- else if(direction == 3){
- return y-1;
- }
- else if(direction == 4){
- return y;
- }
- return 0;
- }
- int check_positionX(int direction,int x,int y){
- if(direction == 1){
- return x-1;
- }
- else if(direction == 2){
- return x;
- }
- else if(direction == 3){
- return x;
- }
- else if(direction == 4){
- return x+1;
- }
- return 0;
- }
- void forward(){
- int a = check_right();
- int b = check_left();
- b = check_left();
- int c = check_right();
- if(b < 10){
- moveMotorTarget(motorB, 50, 15);
- moveMotorTarget(motorC, 50, 0);
- }
- if(a < 10){
- moveMotorTarget(motorC, 50, 15);
- moveMotorTarget(motorB, 50, 0);
- }
- if(a >= 14 && a < 25){
- moveMotorTarget(motorB, 50, 15);
- moveMotorTarget(motorC, 50, 0);
- }
- if(b >= 14 && b < 25){
- moveMotorTarget(motorC, 50, 15);
- moveMotorTarget(motorB, 50, 0);
- }
- wait1Msec(500);
- checkline();
- walkfromline();
- }
- void backward(){
- int a = check_right();
- int b = check_left();
- b = check_left();
- int c = check_right();
- if(b < 10){
- moveMotorTarget(motorB, 30, -15);
- moveMotorTarget(motorC, 30, 0);
- }
- if(a < 10){
- moveMotorTarget(motorC, 30, -15);
- moveMotorTarget(motorB, 30, 0);
- }
- if(a >= 14 && a < 25){
- moveMotorTarget(motorB, 30, -15);
- moveMotorTarget(motorC, 30, 0);
- }
- if(b >= 14 && b < 25){
- moveMotorTarget(motorC, 30, -15);
- moveMotorTarget(motorB, 30, 0);
- }
- wait1Msec(500);
- checklineback();
- walkbackfromline();
- }
- void turn_right(){
- resetMotorEncoder(motorB);
- resetMotorEncoder(motorC);
- checkSoundFile("Turn");
- wait1Msec(200);
- checkSoundFile("Right");
- wait1Msec(500);
- checkSoundFile("Speed up");
- while(getMotorEncoder(motorB) < 200) {
- setMotorSpeed(motorB,30);
- setMotorSpeed(motorC,-30);
- }
- setMotorSpeed(motorB,0);
- setMotorSpeed(motorC,0);
- wait1Msec(1000);
- }
- void turn_left(){
- resetMotorEncoder(motorB);
- resetMotorEncoder(motorC);
- checkSoundFile("Turn");
- wait1Msec(200);
- checkSoundFile("Left");
- wait1Msec(500);
- checkSoundFile("Speed up");
- while(getMotorEncoder(motorC) < 200){
- setMotorSpeed(motorB,-30);
- setMotorSpeed(motorC,30);
- }
- setMotorSpeed(motorB,0);
- setMotorSpeed(motorC,0);
- wait1Msec(1000);
- }
- void check_direction(int init_direction,int direction){
- if(direction == 1){
- if(init_direction == 2){
- turn_right();
- }else if(init_direction == 3){
- turn_left();
- }else if(init_direction == 4){
- turn_right();
- turn_right();
- }
- }
- else if(direction == 2){
- if(init_direction == 1){
- turn_left();
- }else if(init_direction == 3){
- turn_right();
- turn_right();
- }else if(init_direction == 4){
- turn_right();
- }
- }
- else if(direction == 4){
- if(init_direction == 1){
- turn_right();
- turn_right();
- }else if(init_direction == 2){
- turn_left();
- }else if(init_direction == 3){
- turn_right();
- }
- }
- else if(direction == 3){
- if(init_direction == 1){
- turn_right();
- }else if(init_direction == 2){
- turn_right();
- turn_right();
- }else if(init_direction == 4){
- turn_left();
- }
- }
- }
- void assign_map(int nxt_position,int direction,int x,int y){
- if(direction == 1){
- if(nxt_position == 1){
- map_up[x][y] = 1;
- map_down[x-1][y] = 1;
- }else if(nxt_position == 2){
- map_right[x][y] = 1;
- map_left[x][y+1] = 1;
- }else if(nxt_position == 3){
- map_left[x][y] = 1;
- map_right[x][y-1] = 1;
- }
- }else if(direction == 2){
- if(nxt_position == 1){
- map_right[x][y] = 1;
- map_left[x][y+1] = 1;
- }else if(nxt_position == 2){
- map_down[x][y] = 1;
- map_up[x+1][y] = 1;
- }else if(nxt_position == 3){
- map_up[x][y] = 1;
- map_down[x-1][y] = 1;
- }
- }else if(direction == 3){
- if(nxt_position == 1){
- map_left[x][y] = 1;
- map_right[x][y-1] = 1;
- }else if(nxt_position == 2){
- map_up[x][y] = 1;
- map_down[x-1][y] = 1;
- }else if(nxt_position == 3){
- map_down[x][y] = 1;
- map_up[x+1][y] = 1;
- }
- }else if(direction == 4){
- if(nxt_position == 1){
- map_down[x][y] = 1;
- map_up[x+1][y] = 1;
- }else if(nxt_position == 2){
- map_left[x][y] = 1;
- map_right[x][y-1] = 1;
- }else if(nxt_position == 3){
- map_right[x][y] = 1;
- map_left[x][y+1] = 1;
- }
- }
- }
- void travel(int x,int y,int direction){ //direction 1=up 2=right 3=left 4=down
- int first_direction,next_positionX,next_positionY,degree;
- int chk=0;
- if(check_map[x][y] == 1) {return;}
- check_map[x][y] = 1;
- available_block++;
- first_direction = direction;
- degree = getGyroDegrees(S2);
- displayBigTextLine(2,"Available blocks");
- displayBigTextLine(4," %d",available_block);
- if(getUSDistance(S4) > 25){
- next_positionX = check_positionX(first_direction,x,y);
- next_positionY = check_positionY(first_direction,x,y);
- if(next_positionX < 0 || next_positionX >= n || next_positionY < 0 || next_positionY >= n){}
- else {
- assign_map(1,first_direction,x,y);
- if(check_map[next_positionX][next_positionY] == 1) {}
- else {
- forward();
- travel(next_positionX,next_positionY,first_direction);
- }
- }
- }
- check_direction(first_direction,direction);
- int a = check_right();
- int b = check_left();
- if(a > 25){
- if(first_direction == 1){
- direction = 2;
- }
- else if(first_direction == 2){
- direction = 4;
- }
- else if(first_direction == 3){
- direction = 1;
- }
- else if(first_direction == 4){
- direction = 3;
- }
- next_positionX = check_positionX(direction,x,y);
- next_positionY = check_positionY(direction,x,y);
- if(next_positionX < 0 || next_positionX >= n || next_positionY < 0 || next_positionY >= n){}
- else {
- assign_map(2,first_direction,x,y);
- if(check_map[next_positionX][next_positionY] == 1){
- chk = 0;
- }
- else {
- chk = 1;
- turn_right();
- forward();
- travel(next_positionX,next_positionY,direction);
- }
- }
- }
- if(chk == 1) check_direction(first_direction,direction);
- chk = 0;
- a = check_left();
- b = check_right();
- if(a > 25){
- if(first_direction == 1){
- direction = 3;
- }
- else if(first_direction == 2){
- direction = 1;
- }
- else if(first_direction == 3){
- direction = 4;
- }
- else if(first_direction == 4){
- direction = 2;
- }
- next_positionX = check_positionX(direction,x,y);
- next_positionY = check_positionY(direction,x,y);
- if(next_positionX < 0 || next_positionX >= n || next_positionY < 0 || next_positionY >= n){}
- else {
- assign_map(3,first_direction,x,y);
- if(check_map[next_positionX][next_positionY] == 1){
- chk = 0;
- }
- else {
- chk = 1;
- turn_left();
- forward();
- travel(next_positionX,next_positionY,direction);
- }
- }
- }
- if(chk == 1) check_direction(first_direction,direction);
- if(x == 8 && y == 8) {}
- else backward();
- }
- void shortest(int x,int y,int _X,int _Y,int path){
- if(path > short_map[x][y]) {
- return;
- }
- if(path <= short_map[x][y]){
- short_map[x][y] = path;
- }
- if(map_up[x][y] == 1){
- shortest(x-1,y,_X,_Y,path+1);
- }
- if(map_right[x][y] == 1){
- shortest(x,y+1,_X,_Y,path+1);
- }
- if(map_left[x][y] == 1){
- shortest(x,y-1,_X,_Y,path+1);
- }
- if(map_down[x][y] == 1){
- shortest(x+1,y,_X,_Y,path+1);
- }
- }
- void find_temp(int x,int y,int _X,int _Y,int path){
- if(chk_finish == 1) return;
- position_temp[path][0] = x;
- position_temp[path][1] = y;
- if(short_map[x][y] == 0){
- chk_finish = 1;
- return;
- }
- if(x-1 >= 0 && short_map[x-1][y] == path-1 && map_up[x][y] == 1){
- find_temp(x-1,y,_X,_Y,path-1);
- }
- if(y+1 < n && short_map[x][y+1] == path-1 && map_right[x][y] == 1){
- find_temp(x,y+1,_X,_Y,path-1);
- }
- if(y-1 >=0 && short_map[x][y-1] == path-1 && map_left[x][y] == 1){
- find_temp(x,y-1,_X,_Y,path-1);
- }
- if(x+1 < n && short_map[x+1][y] == path-1 && map_down[x][y] == 1){
- find_temp(x+1,y,_X,_Y,path-1);
- }
- }
- void rotate(int nowDirec, int nxtDirec){
- if(nxtDirec == 1){
- if(nowDirec == 2){
- turn_left();
- }else if(nowDirec == 3){
- turn_right();
- }else if(nowDirec == 4){
- turn_right();
- turn_right();
- }
- }else if(nxtDirec == 2){
- if(nowDirec == 1){
- turn_right();
- }else if(nowDirec == 3){
- turn_right();
- turn_right();
- }else if(nowDirec == 4){
- turn_left();
- }
- }else if(nxtDirec == 3){
- if(nowDirec == 1){
- turn_left();
- }else if(nowDirec == 2){
- turn_right();
- turn_right();
- }else if(nowDirec == 4){
- turn_right();
- }
- }else if(nxtDirec == 4){
- if(nowDirec == 1){
- turn_right();
- turn_right();
- }else if(nowDirec == 3){
- turn_right();
- }else if(nowDirec == 2){
- turn_left();
- }
- }
- }
- task main()
- {
- int x=4,y=4;
- setSoundVolume(90);
- checkSoundFile("Motor start");
- wait1Msec(1000);
- checkSoundFile("Motor start");
- wait1Msec(1000);
- checkSoundFile("Start up");
- resetGyro(S2);
- resetMotorEncoder(motorB);
- resetMotorEncoder(motorC);
- resetMotorEncoder(motorA);
- chk_finish = 0;
- for(int i=0;i<n;i++){
- for(int j=0;j<n;j++){
- check_map[i][j] = 0;
- map_up[i][j] = 0;
- map_down[i][j] = 0;
- map_left[i][j] = 0;
- map_right[i][j] = 0;
- short_map[i][j] = 99;
- }
- }
- travel(n-1,n-1,1);
- checkSoundFile("Stop");
- checkSoundFile("One");
- checkSoundFile("Two");
- checkSoundFile("Three");
- wait1Msec(1000);
- checkSoundFile("Comfirm");
- wait1Msec(500);
- checkSoundFile("Start");
- wait1Msec(1000);
- shortest(n-1,n-1,n/2,n/2,0);
- find_temp(x,y,8,8,short_map[x][y]);
- int direction = 1;
- for(int i=0;i<short_map[x][y];i++){
- if(position_temp[i][0] == position_temp[i+1][0] && position_temp[i][1]+1 == position_temp[i+1][1]){
- if(direction != 2) {
- rotate(direction, 2);
- wait1Msec(2000);
- }
- direction = 2;
- }else if(position_temp[i][0] == position_temp[i+1][0] && position_temp[i][1]-1 == position_temp[i+1][1]){
- if(direction != 3) {
- rotate(direction, 3);
- wait1Msec(2000);
- }
- direction = 3;
- }else if(position_temp[i][0]-1 == position_temp[i+1][0] && position_temp[i][1] == position_temp[i+1][1]){
- if(direction != 1) {
- rotate(direction, 1);
- wait1Msec(2000);
- }
- direction = 1;
- }else if(position_temp[i][0]+1 == position_temp[i+1][0] && position_temp[i][1] == position_temp[i+1][1]){
- if(direction != 4) {
- rotate(direction, 4);
- wait1Msec(2000);
- }
- direction = 4;
- }
- forward();
- wait1Msec(2000);
- }
- for(int i=0;i<n;i++){
- displayTextLine(i+2,"%3d%3d%3d%3d%3d%3d%3d%3d%3d",short_map[i][0],short_map[i][1],short_map[i][2],short_map[i][3],short_map[i][4],short_map[i][5],short_map[i][6],short_map[i][7],short_map[i][8]);
- }
- displayTextLine(12," Available Blocks = %d",available_block);
- displayTextLine(13," Shortest Path to (%d,%d) = %d",x,y,short_map[x][y]);
- checkSoundFile("Cheering");
- checkSoundFile("Game over");
- wait1Msec(500);
- checkSoundFile("Elephant call");
- checkSoundFile("Stop");
- wait1Msec(600000);
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement