Joe Code

static float robotspeed=0.0;//speed of the robot's movement
static float robotangle=0.0;//angle of the robot's movement

//static float robotrotation=0.0;//the robot's orentiation
static float robotrotationalvelocity=0.0;//change in the robot's orientation, from -1 to 1

struct {
  float speeds;
  int arrayLength;
} motorSpeeds;

void setMotorSpeeds()//this assumes 4 motors placed on axes.
{
  float vlength=robotspeed;
  float vangle=robotangle;
//	float xmovement=vlength*cosDegrees(vangle-robotrotation);
//	float ymovement=vlength*sinDegrees(vangle-robotrotation);
	float xmovement=vlength*cosDegrees(vangle);
	float ymovement=vlength*sinDegrees(vangle);
	float northValue;
	float southValue;
	float eastValue;
	float westValue;
	nxtDisplayTextLine(5, "xm=%f", xmovement);
	nxtDisplayTextLine(6, "ym=%f", ymovement);
	nxtDisplayTextLine(7, "rrv=%f", robotrotationalvelocity);
	northValue=xmovement-robotrotationalvelocity;
	motor[north]=northValue;
	southValue=-xmovement-robotrotationalvelocity;
	motor[south]=southValue;
	eastValue=ymovement-robotrotationalvelocity;
	motor[east]=eastValue;
	westValue=-ymovement-robotrotationalvelocity;
	motor[west]=westValue;
	nxtDisplayTextLine(1, "north = %f", northValue);
	nxtDisplayTextLine(2, "south = %f", southValue);
	nxtDisplayTextLine(3, "east = %f", eastValue);
	nxtDisplayTextLine(4, "west = %f", westValue);
}

void getMotorSpeeds(motorSpeeds dest)//this assumes 4 motors placed on axes.
{
  float vlength=robotspeed;
  float vangle=robotangle;
//	float xmovement=vlength*cosDegrees(vangle-robotrotation);
//	float ymovement=vlength*sinDegrees(vangle-robotrotation);
	float xmovement=vlength*cosDegrees(vangle);
	float ymovement=vlength*sinDegrees(vangle);

	float speedValues[4];
  speedValues[0]=xmovement + sinDegrees(robotrotationalvelocity);
	speedValues[1]=-xmovement - sinDegrees(robotrotationalvelocity);
	speedValues[2]=ymovement + cosDegrees(robotrotationalvelocity);
	speedValues[3]=-ymovement - cosDegrees(robotrotationalvelocity);

	dest.speeds=speedValues;
	dest.arrayLength=4;
}

void rotateVector(float* vlength, float* vangle, float rotation)//rotates the vector given by vlength and vangle by rotation.
{
	vangle=vangle+rotation;
	if(vangle>360.0)
	{
		vangle-=360;
	}
	if(vangle<-360.0)
	{
		vangle+=360;
	}
}

void increaseVector(float* vlength1, float* vangle1, float vlength2, float vangle2)//adds vector2 to vector1 storing in vector1
{
	float x1=vlength1*cosDegrees(vangle1);
	float y1=vlength1*sinDegrees(vangle1);
	x1+=vlength2*cosDegrees(vangle2);
	y1+=vlength2*sinDegrees(vangle2);
	//TODO: this!
/*	*vlength1=asin()/
	if()
	{

	}
	*vangle1=atansin()
*/
}

void setVelocity(int speed, float angle)
{
	robotspeed=speed;
	robotangle=angle;
}

void setRotationalVelocity(float turnVelocity)
{
	robotrotationalvelocity=turnVelocity;
}