//
// Author: Michael Zarozinski
// E-mail: MichaelZ@LouderThanABomb.com
// Robot name: "SparkTutorial01"
// Robot version: 1.0
// Driver function: SparkTutorial01()
// Filename: SparkTutorial01.cpp
// Release date: 01/01
//
// include this before others to avoid windows.h problems
#include "SparkModel.h"
#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <math.h>
#include "car.h"
#include "track.h"
#include "misc.h"
// Use namespaces incase we add additional Spark! models in the future
#define USE_SPARK_NAMESPACE
#include "steering.h" // .h exported from Spark!
con_vec SparkTutorial01( situation& s )
{
const char name[] = "SparkTutorial01"; // This is the robot driver's name!
static SparkModel steering; // model for the steering
double width = s.to_lft + s.to_rgt; // calc the width of the track
static init = 1; // flag to indicate it's our first time through
con_vec result; // control vector
double alpha, vc; // components of result
if( init )
{
// first time only, copy name:
my_name_is(name);
result.alpha = result.vc = 0;
// init the Spark! model
steering.load("./rarsai/steering.spk", true); // open in debug mode
const char* err = steering.get_msg_textA();
// if there was an error, report it
if (err != NULL)
{
MessageBox(NULL, err, "Error", MB_OK);
fprintf(stderr, err );
exit( 1 );
}
init = 0; // make sure we don't come back in here
return result;
} // end if starting
// "Cheat" function - this will take over and get us back "on track" if we've
// wantered off the track
if (stuck(s.backward,s.v,s.vn,s.to_lft,s.to_rgt,&result.alpha,&result.vc))
return result;
// calc position on the track - as a % from the left side of the track
float value = (s.to_lft/width) * 100.0;
// set the current_position value...
steering.set_value(STEERING::CURRENT_POSITION, value);
// Set "facing" to be vn. It's the component of v which is perpendicular
// to the track walls. This tells you if you are drifting toward a wall,
// and how fast. You need to keep this from getting very large or very
// negative or you will go off the track soon afterward. Negative means
// heading toward right wall.
steering.set_value(STEERING::FACING, s.vn);
// get the output...
alpha = steering.get_cog_output();
// set the steering angle...
result.alpha = alpha;
// fix the desired speed
vc = 40;
result.vc = vc;
return result;
} // end SparkTutorial01
|
