Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- #include "simpletools.h" // Include simpletools header
- #include "ping.h" // Include ping header
- #include "abdrive.h" // Include abdrive header
- #define SIDE 40
- #define NORTH 0
- #define EAST 1
- #define SOUTH 2
- #define WEST 3
- #define cmToTicks (1 / 0.325)
- #define STOP 950
- #define TURN 1000
- #define speed 50
- #define factor 2
- int currentDirection = NORTH;
- void turnDirection(int direction){
- drive_ramp(30, 30);
- int diff = currentDirection - direction;
- drive_ramp(-(30 * diff), (30 * diff));
- pause(TURN * abs(diff));
- currentDirection = direction;
- drive_ramp(128, 128);
- return;
- }
- int checkNext(int input[], int index, int size){
- if (index >= size - 1){
- return -1;
- }
- int curr = input[index];
- int target = input[index + 1];
- int diff = target - curr;
- //print("%d\n", diff);
- return diff;
- }
- void move(int input[], int index, int sizeOfInput){
- int diff = checkNext(input, index, sizeOfInput);
- if(diff == -1){
- drive_ramp(0, 0);
- return;
- }
- int counter = 0;
- switch (diff){
- //move right until diff is 1
- case 1:
- turnDirection(EAST);
- counter = 0;
- while(diff == 1){
- ++index;
- counter++;
- diff = checkNext(input, index, sizeOfInput);
- }
- drive_ramp(128, 128);
- pause(counter * STOP);
- break;
- //move left until diff is -1
- case -1:
- turnDirection(WEST);
- counter = 0;
- while(diff == -1){
- ++index;
- counter++;
- diff = checkNext(input, index, sizeOfInput);
- }
- drive_ramp(128, 128);
- pause(counter * STOP);
- break;
- //move forward until diff is 4
- case 4:
- turnDirection(NORTH);
- counter = 0;
- while(diff == 4){
- ++index;
- counter++;
- diff = checkNext(input, index, sizeOfInput);
- }
- drive_ramp(128, 128);
- pause(counter * STOP);
- break;
- //move backwards until diff is -4
- default:
- turnDirection(SOUTH);
- counter = 0;
- while(diff == -4){
- ++index;
- counter++;
- diff = checkNext(input, index, sizeOfInput);
- }
- drive_ramp(128, 128);
- pause(counter * STOP);
- }
- move(input, index, sizeOfInput);
- }
- int main(){
- int inp[] = {-3, 1, 5, 6, 7, 11, 15, 16};
- int len = sizeof(inp)/sizeof(inp[0]);
- drive_ramp(128, 128);
- move(inp, 0, len);
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement