int main() // main function
{
// Proportional control
servo_angle(16, 650); // 7 ticks/sec / 20 ms
while(1) // main loop
{
//pause(200);
distance = ping_cm(17);
//print("distance= %d\n", distance);
pause(200);
if (distance >25)
{
drive_speed(75,75);
}
else
{
drive_speed(0,0);
servo_angle(16, 10);
pause(1000);
int distanceleft = ping_cm(17);
pause(1000);
//print("distance left= %d\n", distanceleft);
servo_angle(16, 1600);
pause(1000);
//print("distance= %d\n", distanceleft);
int distanceright = ping_cm(17);
pause(1000);
//print("distance right= %d\n", distanceright);
servo_angle(16, 650);
pause(1000);
//print("distance= %d\n", distanceleft);
//servo_angle(16, 750);
int a = distanceleft - distanceright;
//pause(200);
//print("a= %d\n", a);
pause(200);
if (a == 0)
{
drive_speed(64,-64);
pause(300);
print("1\n");
}
else if (a > 0)
{
drive_speed(-64,64);
pause(150);
print("2\n");
}
else if(a < 0)
{
drive_speed(64,-64);
pause(150);
print("3\n");
}
}
// Calculate correction speed
}
}
int main()
{
//drive_pins(14, 15, 12, 13);
drive_goto(100, 100);
drive_speed(0, 0);
#ifdef interactive_development_mode
drive_record(1);
#endif
pause(300);
drive_speed(20, 20);
pause(1600);
#ifdef interactive_development_mode
drive_record(0);
#endif
pause(6400);
#ifdef interactive_development_mode
drive_record(1);
#endif
pause(1600);
drive_speed(0, 0);
#ifdef interactive_development_mode
drive_record(0);
#endif
#ifdef interactive_development_mode
drive_displayControlSystem(0, 2500/11);
#endif
while(1);
}
int main()
{
while(1) // se crea un ciclo infinito
{
distance1 = ping_cm(8); //el sensor frontal va conectado al P8 de la placa
distance2 = ping_cm(4); //el sensor lateral va conectado al P4 de la placa
if(distance2<15){ //declara que la distancia, reconocida por el sensor lateral, sera de 15 cm
if(distance1<10){ //declara que la distancia, reconocida por el sensor frontal, sera de 10 cm
drive_speed(0, 0); //si se encuentra un obstaculo, el robot para
drive_goto(-25, 26); //luego girara hacia la izquierda de acuerdo al algoritmo wall follower
pause(200);
}
else{
drive_speed(64, 64); //de no encontrar obstaculo, el robot sigue avanzando
pause(100);
}
}
else{
drive_speed(0, 0);
drive_goto(18, 18);
pause(200);
drive_goto(26, -25); //en caso que exista un camino sin salida, gira hacia la derecha.
pause(200);
drive_goto(30, 30);
pause(200);
}
}
}
int main() // main function
{
drive_setRampStep(10); // 10 ticks/sec / 20 ms
drive_ramp(128, 128); // Forward 2 RPS
// While disatance greater than or equal
// to 20 cm, wait 5 ms & recheck.
while(ping_cm(8) >= 20) pause(5); // Wait until object in range
drive_ramp(0, 0); // Then stop
// Turn in a random direction
turn = rand() % 2; // Random val, odd = 1, even = 0
if(turn == 1) // If turn is odd
drive_speed(64, -64); // rotate right
else // else (if turn is even)
drive_speed(-64, 64); // rotate left
// Keep turning while object is in view
while(ping_cm(8) < 20); // Turn till object leaves view
drive_ramp(0, 0); // Stop & let program end
}
int main() // Main function
{
rfid = rfid_open(rfidSout, rfidEn); // Open reader, start reading
while(1) // Main loop
{
char *str = rfid_get(rfid, 1000); // Wait up to 1 s for card
if(!strcmp(str, "timed out")) { // Timed out?
print("No ID scanned.\n"); // display "No ID..."
drive_speed(0,0);
}
else if(!strcmp(str, "1E009BC215")) { // Tag A ID match?
print("Tag A detected.\n"); // display "Tag A..."
drive_speed(64,64);
}
else if(!strcmp(str, "1E009D9881")){ // Tag B ID match?
print("Tag B detected.\n"); // display "Tag B..."
drive_speed(-64,-64);
}
else if(!strcmp(str, "16001A00DC")) {
print("Tag C detected.\n");
drive_speed(-32,32);
}
else if(!strcmp(str, "010DBEA632")){
print("Tag D detected.\n");
drive_speed(32,-32);
}
else // No matches?
print("Unknown ID = %s.\n", str); // print ID.
}
}
int main()
{
int distance_inches=0;
lcd_init(0);
pause(100);
lcd_clear();
while(1)
{
pause(30);
lcd_clear();
distance_inches=ping_inches(8);
if (distance_inches < 10)
{
lcd_print_number(distance_inches);
lcd_write_char(':');
lcd_write_char('(');
pause(100);
drive_speed(27,-27);
lcd_clear();
}
else
drive_speed(0,0);
}
}
int main() // Main function
{
xbee = fdserial_open( 9, 8, 0, 9600 ); //Initialize communication
char c; //Create char
while ( 1 )
{
c = fdserial_rxChar( xbee ); //get all values sent to the ActivityBot
if ( c == 'f' ) //if the robot was told to go forward, go forward
{
drive_speed( 64, 64 );
} else if ( c == 'b' ) //if the robot was told to go backward, go backward
{
drive_speed( -64, -64 );
} else if ( c == 'l' ) //if the robot was told to go left, go left
{
drive_speed( 0, 64 );
} else if ( c == 'r' ) //if the robot was told to go right, go right
{
drive_speed( 64, 0 );
} else if ( c == 's' ) //if the robot is told to stop, stop
{
drive_speed( 0, 0 );
}
}
}
int main() // Main function
{
// Add startup code here.
//dac_ctr(26, 0, 0);
//dac_ctr(27, 1, 0);
low(26);
low(27);
while(1)
{
/*
freqout(10, 1, 38000);
irLeft = input(11);
pause(10);
low(10);
freqout(1, 1, 38000);
irRight = input(2);
pause(10);
low(1);
*/
distance = ping_cm(ULTRASONIC);
if ((distance > 15)) {
print("%c Distancia = %d .. Seguir adelante",
HOME, distance);
drive_speed(60, 60);
} else if (irLeft == 1){
//print("%c irLeft = %d, irRight = %d .. Rotar izquierda",
//HOME, irLeft, irRight);
//drive_speed(0, 26);
//pause(1000);
//drive_speed(0, 0);
} else if (distance < 15) {
print("%c Distancia = %d .. Girar izquierda",
HOME, distance);
drive_speed(0, 51);
pause(1000);
drive_speed(0, 0);
pause(500);
} else {
//print("%c irLeft = %d, irRight = %d .. Ni idea",
//HOME, irLeft, irRight);
// drive_speed(52, 0);
//pause(2000);
//drive_speed(0, 0);
}
/*
print("%c distance = %d%c cm",
HOME, distance, CLREOL);
*/
//print("%c irLeft = %d, irRight = %d", HOME, irLeft, irRight);
pause(50);
}
}
void id_bot(struct RoboAI *ai, struct blob *blobs)
{
// Drive a simple pattern forward/reverse and call the particle filter to
// find the blobs that are consistent with this in order to identify the
// bot as well as the opponent.
double noiseV=5; // Expected motion magnitude due to noise for a static object
// (it's just a guess - haven't measured it)
double oppSizeT=2500; // Opponent's blob nimimum size threshold
static double blobData[10][4]; // Keep track of up to 10 blobs that could be
// the bot. Unlikely there are more since
// these have to be active tracked blobs.
// blobData[i][:]=[blobID mx my p]
static int bdataidx=0;
double psum;
double px,siz,len,vx,vy,wx,wy;
int ownID,i;
struct blob *oppID;
struct blob *p;
static double IDstep=0; // Step tracker
double frameInc=1.0/10; // 1 over the number of frames each step should get
// Get a handle on the ball
id_ball(ai,blobs);
// Forward motion for a few frames
if (IDstep<3*frameInc)
{
drive_speed(35);
clear_motion_flags(ai);
ai->st.mv_fwd=1;
}
else if (IDstep<1.0)
{
drive_speed(35);
clear_motion_flags(ai);
ai->st.mv_fwd=1;
particle_filter(ai,blobs);
}
else if (IDstep<1.0+(3*frameInc)) // Reverse drive
{
drive_speed(-35);
clear_motion_flags(ai);
ai->st.mv_back=1;
}
else if (IDstep<2.0)
{
drive_speed(-35);
clear_motion_flags(ai);
ai->st.mv_back=1;
particle_filter(ai,blobs);
}
else if (IDstep<2.0+(5*frameInc)) {clear_motion_flags(ai); all_stop();} // Stop and clear blob motion history
else {IDstep=0; ai->st.state=1;} // For debug...
IDstep+=frameInc;
}
void correct() {
if(ping_cm(8) < 14) { // if too close, move back
while(ping_cm(8) < 14) {
drive_speed(-10,-10);
pause(10);
}
} else {
while(ping_cm(8) > 14) { // if too far, move forward
drive_speed(10,10);
pause(10);
}
}
drive_speed(0,0);
}
int main() // Main - execution begins!
{
xbee = fdserial_open(11, 10, 0, 9600);
//dprint(xbee, "hello");
//dprint(xbee, "hello");
drive_speed(0,0); // Start servos/encoders cog
drive_setRampStep(10); // Set ramping at 10 ticks/sec per 20 ms
sirc_setTimeout(50); // Remote timeout = 50 ms
//drive_feedback(0);
dt = CLKFREQ/10;
t = CNT;
while(1) // Outer loop
{
int button = sirc_button(4); // check for remote key press
// Motion responses - if key pressed, set wheel speeds
if(button == 2)
{
if(!running)
cogstart(&gofwd, NULL, fstack, sizeof fstack);
}
if(button == 8)
{
if(!running)
cogstart(&gobkwd, NULL, bstack, sizeof bstack);
}
if(button == CH_UP)drive_rampStep(100, 100); // Left turn
if(button == CH_DN)drive_rampStep(-100, -100); // Right turn
if(button == VOL_DN)drive_rampStep(-100, 100); // Left turn
if(button == VOL_UP)drive_rampStep(100, -100); // Right turn
if(button == MUTE)drive_rampStep(0, 0); // Stop
if(button == ENTER)
{
getTicks();
displayTicks();
}
if(CNT - t > dt)
{
t+=dt;
i++;
getTicks();
if
(
ticksLeftCalc != ticksLeftCalcOld
|| ticksRightCalc != ticksRightCalcOld
|| ticksLeft != ticksLeftOld
|| ticksRight != ticksRightOld
)
{
displayTicks();
}
}
}
}
int turn_180(){
drive_speed(0,0);
pause(100);
drive_goto(-50,50);
pause(200);
return 1;
}
static void Config()
{
// Initialize the drive speed, set the maximum speed, and set ramp step
drive_speed(0, 0);
drive_setMaxSpeed(DEFAULT_MAX_SPEED);
// Note: Chris has a question as to whether the ramping needed to be adjusted. This needs to be addressed.
drive_setRampStep(3);
}
void moveForwardSquare()
{
double xDist = 0;
double yDist = 0;
double angle = 0;
double distance = 0;
int prevTicksLeft = 0;
int prevTicksRight = 0;
drive_getTicks(&prevTicksLeft, &prevTicksRight);
while(distance < GRID_SIZE) {
getIR();
int changeVal;
if(irLeft < SENSOR_VALUE && irRight < SENSOR_VALUE){ // Wall either side
changeVal = (irLeft - irRight) * MULTIPLIER;
} else if ( irLeft < SENSOR_VALUE ) { // Wall to the left
changeVal = (irLeft - IR_LEFT) * MULTIPLIER;
} else if ( irRight < SENSOR_VALUE ) { // Wall to the right
changeVal = (IR_RIGHT - irRight) * MULTIPLIER;
} else { // If no walls to the side, carry on as normal
changeVal = 0;
}
drive_speed(BASE_SPEED_TICKS - changeVal, BASE_SPEED_TICKS + changeVal); // Set the new drive speed with the new changeVal
int ticksLeft, ticksRight;
drive_getTicks(&ticksLeft, &ticksRight); // get current ticks count
// calculate distances
double distRight = calcDistance(ticksRight, prevTicksRight); // Distance of left wheel
double distLeft = calcDistance(ticksLeft, prevTicksLeft); // Distance of right wheel
double distCentre = (distRight + distLeft) / 2; // The average of the left and right distance
angle = angle + (distRight - distLeft) / ROBOT_WIDTH;
// update prevTicks
prevTicksLeft = ticksLeft;
prevTicksRight = ticksRight;
xDist = xDist + distCentre * cos(angle);
yDist = yDist + distCentre * sin(angle);
distance = sqrt(xDist*xDist + yDist*yDist); // work out distance travelled using pythagoras
}
drive_speed(0,0);
}
int turn_left(){
drive_speed(0,0);
pause(100);
drive_goto(25,25);
drive_goto(-26,26);
pause(100);
drive_goto(75,75);
//drive_goto(50,50);
return 1;
}
int turn_rigt(){
drive_speed(0,0);
pause(100);
drive_goto(25,25);
drive_goto(26,-26);
pause(100);
drive_goto(75,75);
//drive_goto(50,50);
return 0;
}
int main() // Main function
{
int a;
sirc_setTimeout(1000);
while(1)
{
a=sirc_button(3);
if(a==16)
drive_speed(128,128); //Move forward
else if(a==17)
drive_speed(-128,-128); //Move backward
else if(a==18)
drive_goto(50,0); //Move Right
else if(a==19)
drive_goto(0,50); //Move left
else if(a==20)
drive_speed(0,0); //Stop moving
pause(100);
}
}
void kickBall(struct RoboAI* ai) {
all_stop();
printf("KICKING\n");
kick_speed(100);
drive_speed(100);
sleep(2);
stop_kicker();
all_stop();
// go to reset state
ai->st.state++;
}
void id_bot(struct RoboAI *ai, struct blob *blobs)
{
///////////////////////////////////////////////////////////////////////////////
// ** DO NOT CHANGE THIS FUNCTION **
// This routine calls track_agents() to identify the blobs corresponding to the
// robots and the ball. It commands the bot to move forward slowly so heading
// can be established from blob-tracking.
//
// NOTE 1: All heading estimates, velocity vectors, position, and orientation
// are noisy. Remember what you have learned about noise management.
//
// NOTE 2: Heading and velocity estimates are not valid while the robot is
// rotating in place (and the final heading vector is not valid either).
// To re-establish heading, forward/backward motion is needed.
//
// NOTE 3: However, you do have a reliable orientation vector within the blob
// data structures derived from blob shape. It points along the long
// side of the rectangular 'uniform' of your bot. It is valid at all
// times (even when rotating), but may be pointing backward and the
// pointing direction can change over time.
//
// You should *NOT* call this function during the game. This is only for the
// initialization step. Calling this function during the game will result in
// unpredictable behaviour since it will update the AI state.
///////////////////////////////////////////////////////////////////////////////
struct blob *p;
static double stepID=0;
double frame_inc=1.0/5.0;
drive_speed(30); // Start forward motion to establish heading
// Will move for a few frames.
track_agents(ai,blobs); // Call the tracking function to find each agent
if (ai->st.selfID==1&&ai->st.self!=NULL)
fprintf(stderr,"Successfully identified self blob at (%f,%f)\n",ai->st.self->cx,ai->st.self->cy);
if (ai->st.oppID==1&&ai->st.opp!=NULL)
fprintf(stderr,"Successfully identified opponent blob at (%f,%f)\n",ai->st.opp->cx,ai->st.opp->cy);
if (ai->st.ballID==1&&ai->st.ball!=NULL)
fprintf(stderr,"Successfully identified ball blob at (%f,%f)\n",ai->st.ball->cx,ai->st.ball->cy);
stepID+=frame_inc;
if (stepID>=1&&ai->st.selfID==1) // Stop after a suitable number of frames.
{
ai->st.state+=1;
stepID=0;
all_stop();
}
else if (stepID>=1) stepID=0;
// At each point, each agent currently in the field should have been identified.
return;
}
void Robot::setSpeed(int left, int right)
{
#ifdef DEBUG
Serial.print("L ");
Serial.print(m_wheelLeft);
Serial.print(", R ");
Serial.println(m_wheelRight);
#endif
m_wheelLeft = left;
m_wheelRight = right;
drive_speed(m_wheelLeft, m_wheelRight);
}
int main() // main function
{
int DO = 22,CLK = 23, DI = 24, CS = 25;
sd_mount(DO,CLK,DI,CS);
wav_volume(10);
freqout(4, 2000, 3000); // Speaker tone: P4, 2 s, 3 kHz
while(1) // Endless loop
{
int wL = input(7); // Left whisker -> wL variable
int wR = input(8); // Right whisker -> wR variable
//print("%c", HOME); // Terminal cursor home (top-left)
//print("wL = %d wR = %d", wL, wR); // Display whisker variables
drive_speed(128,128);
if(input(7)==0 || input(8) == 0)
{
drive_speed(0,0);
pause(500);
wav_play("ouch.wav");
drive_goto(-64,64);
}
}
}
int main(void) // main function
{
int distance;
int addr = 32769; // Pick EEPROM base address.
int side = 1; // 0=Left, 1=Right
int exit = 0;
while(!exit) {
print("Enter direction (0 - forward, 1 - backward, 2 - Exit): "); // User prompt
scan("%d\n", &side);
freqout(4, 250, 3000); //pin, duration, frequency
switch(side)
{
case 0: // forward with turn
high(26);
pause(300);
drive_speed(-25, 26); //90deg Left turn
pause(1000);
drive_speed(0, 0);
break;
case 1: // backward with turn
high(27);
pause(300);
drive_speed(26, -25); //90deg Right turn
pause(1000);
drive_speed(0, 0);
break;
case 2:
print("Exit");
exit = 1;
}
}
print("\xff\x00\x00");
}
int checkClose(){
irLeft = 0;
irRight = 0;
distance = ping_cm(8);
for(int dacVal = 0; dacVal < 160; dacVal += 8){
dac_ctr(26, 0, dacVal);
freqout(11, 1, 38000);
irLeft += input(10);
dac_ctr(27, 1, dacVal);
freqout(1, 1, 38000);
irRight += input(2);
}
if (irLeft<7) {
drive_speed(25,0);
} else if (irLeft > 9 || irLeft == 0){
drive_speed(0,25);
} else {
drive_speed(40,40);
}
if (distance<=5){
fullTurn(90);
degrees+=90;
end=1;
}
drive_getTicks(&ticksLeft, &ticksRight);
leftDeg = ((ticksLeft * 360) / 332.38128) * 3.25;
rightDeg = ((ticksRight * 360) / 332.38128) * 3.25;
degrees = degrees + (rightDeg - leftDeg);
moved = ticksLeft * 3.25;
}
int main() // main function
{
simpleterm_reopen(RX_PIN, TX_PIN, 0, BAUD);
//drive_open();
//drive_goto(0,0);
//drive_setMaxSpeed(MAX_SPEED);
int l=1;
int r=1;
drive_speed(-10,-10);
pause(1000);
drive_speed(0,0);
while(1) // Repeat indefinitely
{
for(int i = 0; i < 100; i++)
{
drive_getTicks(&l,&r); //only works when actually moving, otherwise no response
int ultrasonic = ping(PING_PIN);
//print("ultrasonic %d uS\n",ultrasonic);
print("%d %d\n", l, r);
pause(50);
}
}
}
void derecha(){
for(int n = 1; n <= 65; n++){ // Count to hundred
drive_rampStep(35,35); // move not too fast
pause(10); // 50 ms between reps
}
drive_speed(0,0);
pause(500);
drive_goto(25,-25); //giro a la derecha
pause(500);
for(int n = 1; n <= 65; n++){ // Count to hundred
drive_rampStep(35,35); // move not too fast
pause(10); // 50 ms between reps
}
}
int main() // Main function
{
// Add startup code here.
while(1)
{
freqout(1, 1, 33000); // Repeat for right detector
irRight = input(2);
if(ping_cm(8)>50){ //Salida del laberinto
drive_speed(64,64);
pause (3000);
}
if(irRight == 1){ // Si no hay pared a la derecha
drive_speed(42,42); //Avanza
pause(600);
high(26);
high(27);
//drive_goto(25,-26); //Gira derecha
drive_speed(32,-32);
pause(750);
//drive_speed(0,0);
low(26);
low(27);
drive_speed(42,42); //Avanza
pause(1200);
//drive_speed(0,0);
}
if(irRight == 0 && ping_cm(8)<10){ //Si hay pared a la derecha y enfrente
drive_speed(0,0);
//Gira izquierda
drive_speed(-32,32);
pause(800);
drive_speed(0,0);
}
else{ //Si no hay pared enfrente
drive_speed(42,42);
pause(50);
}
}
}
//static int step = 6;
void drive_rampStep(int left, int right)
{
int leftTemp, rightTemp;
int sprOld = _servoPulseReps;
if(left > abd_speedL + abd_rampStep) leftTemp = abd_speedL + abd_rampStep;
else if(left < abd_speedL - abd_rampStep) leftTemp = abd_speedL - abd_rampStep;
else leftTemp = left;
if(right > abd_speedR + abd_rampStep) rightTemp = abd_speedR + abd_rampStep;
else if(right < abd_speedR - abd_rampStep) rightTemp = abd_speedR - abd_rampStep;
else rightTemp = right;
drive_speed(leftTemp, rightTemp);
while(sprOld >= _servoPulseReps);
sprOld = _servoPulseReps;
}
int main() // Main function
{
while(1) // Main loop
{
distance = ping_cm(8); // Get cm distance from Ping)))
if(distance>30){
int qtis = getQTIs(7, 4); // Check stripe position
if(qtis == 0b1000) drive_speed(-64, 64); // Far left, rotate left
if(qtis == 0b1100) drive_speed(0, 64); // Left, pivot left
if(qtis == 0b0100) drive_speed(32, 64); // A little left, curve left
if(qtis == 0b0110) drive_speed(64, 64); // Stripe centered, forward
if(qtis == 0b0010) drive_speed(64, 32); // A little right, curve right
if(qtis == 0b0011) drive_speed(64, 0); // Right, pivot right
if(qtis == 0b0001) drive_speed(64, -64); // Far right, rotate right
if(qtis == 0b1111) drive_speed(0, 0);
lcd_print_string("station reached");
}
}
}
void id_bot(struct RoboAI *ai, struct blob *blobs)
{
// ** DO NOT CHANGE THIS FUNCTION **
// This routine calls track_agents() to identify the blobs corresponding to the
// robots and the ball. It commands the bot to move forward slowly so heading
// can be established from blob-tracking.
//
// NOTE 1: All heading estimates are noisy.
//
// NOTE 2: Heading estimates are only valid when the robot moves with a
// forward or backward direction. Turning destroys heading data
// (why?)
//
// You should *NOT* call this function during the game. This is only for the
// initialization step. Calling this function during the game will result in
// unpredictable behaviour since it will update the AI state.
struct blob *p;
static double stepID = 0;
double frame_inc = 1.0 / 5.0;
drive_speed(30); // Need a few frames to establish heading
track_agents(ai, blobs);
if (ai->st.selfID == 1 && ai->st.self != NULL)
fprintf(stderr, "Successfully identified self blob at (%f,%f)\n", ai->st.self->cx[0], ai->st.self->cy[0]);
if (ai->st.oppID == 1 && ai->st.opp != NULL)
fprintf(stderr, "Successfully identified opponent blob at (%f,%f)\n", ai->st.opp->cx[0], ai->st.opp->cy[0]);
if (ai->st.ballID == 1 && ai->st.ball != NULL)
fprintf(stderr, "Successfully identified ball blob at (%f,%f)\n", ai->st.ball->cx[0], ai->st.ball->cy[0]);
stepID += frame_inc;
if (stepID >= 1 && ai->st.selfID == 1)
{
ai->st.state += 1;
stepID = 0;
all_stop();
}
else if (stepID >= 1) stepID = 0;
return;
}
int main() // Main function
{
int DO = 22, CLK = 23; // SD I/O pins
int DI = 24, CS = 25;
sd_mount(DO, CLK, DI, CS); // Mount SD file system
fp = fopen("navset.txt", "r"); // Open navset.txt
fread(str, 1, 512, fp); // navset.txt -> str
int strLength = strlen(str); // Count chars in str
int i = 0; // Declare index variable
drive_speed(0, 0); // Speed starts at 0
while(1) // Loop through commands
{
// Parse command
while(!isalpha(str[i])) i++; // Find 1st command char
sscan(&str[i], "%s", cmdbuf); // Command -> buffer
i += strlen(cmdbuf); // Idx up by command char count
if(!strcmp(cmdbuf, "end")) break; // If command is end, break
// Parse distance argument
while(!isdigit(str[i])) i++; // Find 1st digit after command
sscan(&str[i], "%s", valbuf); // Value -> buffer
i += strlen(valbuf); // Idx up by value char count
val = atoi(valbuf); // Convert string to value
// Execute command
if(strcmp(cmdbuf, "forward") == 0) // If forward
drive_goto(val, val); // ...go forward by val
else if(strcmp(cmdbuf, "backward") == 0) // If backward
drive_goto(-val, -val); // ... go backward by val
else if(strcmp(cmdbuf, "left") == 0) // If left
drive_goto(-val, val); // ...go left by val
else if(strcmp(cmdbuf, "right") == 0) // If right
drive_goto(val, -val); // ... go right by val
}
fclose(fp); // Close SD file
}