void turn(int angle) { // use +90 turn right and -90 to turn left
float compassTarget, compassOrigin; //used for turning and configuring compass
float error;
compassOrigin = (float) SensorValue(compass);
compassTarget = (compassOrigin - angle); // in degrees
if(compassTarget<0) //make sure the angle is between 0 and 360
compassTarget += 360;
if(compassTarget>360)
compassTarget -= 360;
error = atan2(sinDegrees(compassTarget)-sinDegrees(compassOrigin),cosDegrees(compassTarget)-cosDegrees(compassOrigin));
motor[leftWheel] = sgn(angle)*(15*error+5);
motor[rightWheel] = -sgn(angle)*(15*error+5);
while(abs(error)<PI/90.0){
motor[leftWheel] = sgn(angle)*(15*error+5);
motor[rightWheel] = -sgn(angle)*(15*error+5);
error = atan2(sinDegrees(compassTarget)-sinDegrees(compassOrigin),cosDegrees(compassTarget)-cosDegrees(compassOrigin));
}
motor[leftWheel] = 0;
motor[rightWheel] = 0;
}
task MotorControl()
{
//SpeedA = (MaxSpeed-abs(Spin))*cosDegrees(Direction-30);
//SpeedB = (MaxSpeed-abs(Spin))*cosDegrees(Direction-150);
//SpeedC = (MaxSpeed-abs(Spin))*cosDegrees(Direction+90);
while (Run ==1)
{
SpeedA = (MaxSpeed-abs(Spin))*cosDegrees(Direction-150)*StopMove;
SpeedB = (MaxSpeed-abs(Spin))*cosDegrees(Direction+90)*StopMove;
SpeedC = (MaxSpeed-abs(Spin))*cosDegrees(Direction-30)*StopMove;
SpeedA = SpeedA+Spin;
SpeedB = SpeedB+Spin;
SpeedC = SpeedC+Spin;
motor[A] = SpeedA;
motor[B] = SpeedB;
motor[C] = SpeedC;
if (SensorValue[Touch] ==1)
{
Run =0;
}
}
}
task MapRoom()
{
float botLastXCoordinate = 0.0;
float botLastYCoordinate = 0.0;
float botCurrentXCoordinate = 0.0;
float botCurrentYCoordinate = 0.0;
int wallXCoordinate = 0;
int wallYCoordinate = 0;
int xCoordinateDisplayOffset = 50;
int yCoordinateDisplayOffset = 40;
startGyro();
resetGyro();
eraseDisplay();
while(true)
{
//Robot position
botCurrentXCoordinate = botLastXCoordinate + EncoderDistance(ReadEncoderValue()) * sinDegrees(readGyro());
botCurrentYCoordinate = botLastYCoordinate + EncoderDistance(ReadEncoderValue()) * cosDegrees(readGyro());
//Wall mapping
wallXCoordinate = botCurrentXCoordinate + ReadSonar(2) * CENTIMETERS_TO_INCHES * cosDegrees(readGyro());
wallYCoordinate = botCurrentYCoordinate + ReadSonar(2) * CENTIMETERS_TO_INCHES * sinDegrees(readGyro());
nxtSetPixel(wallXCoordinate + xCoordinateDisplayOffset, wallYCoordinate + yCoordinateDisplayOffset);
ResetEncoderValue();
botLastXCoordinate = botCurrentXCoordinate;
botLastYCoordinate = botCurrentYCoordinate;
wait1Msec(20);
}
}
void turn(int angle) { // use +90 turn right and -90 to turn left (or any other angle, positive means rigth turns)...
float compassAngle, compassTarget, compassOrigin; //used for turning and configuring compass
float error;
compassOrigin = SensorValue(compass);
compassTarget = (compassOrigin - angle);
if(compassTarget<0) //make sure the angle is between 0 and 360 (adding or subtracting 360 results in the same heading)
compassTarget += 360;
if(compassTarget>360)
compassTarget -= 360;
error = atan2(sinDegrees(compassTarget)-sinDegrees(compassAngle),cosDegrees(compassTarget)-cosDegrees(compassAngle));
motor[leftWheel] = sgn(angle)*15*error;
motor[rightWheel] = -sgn(angle)*15*error;
while(abs(error)<PI/90.0){
motor[leftWheel] = sgn(angle)*15*error;
motor[rightWheel] = -sgn(angle)*15*error;
error = atan2(sinDegrees(compassTarget)-sinDegrees(compassAngle),cosDegrees(compassTarget)-cosDegrees(compassAngle));
}
motor[leftWheel] = 0;
motor[rightWheel] = 0;
}
float calculate_likelihood(float x, float y, float theta, float z)
{
int closestWallIndex = -1;
// We take into consideration the greatest distance
float shortestDistance = 20000.0;
float m;
int i = 0;
for (i = 0; i < NUMBER_OF_WALLS; i++)
{
float a_x = wallAxArray[i], a_y = wallAyArray[i];
float b_x = wallBxArray[i], b_y = wallByArray[i];
m = ((b_y - a_y)*(a_x - x) - (b_x - a_x)*(a_y - y)) / ((b_y - a_y)*cosDegrees(theta) - (b_x - a_x)*sinDegrees(theta));
float px = x + m*cosDegrees(theta);
float py = y + m*sinDegrees(theta);
if( px >= min(a_x,b_x) && px <= max(a_x,b_x) && py >= min(a_y,b_y) && py <= max(a_y,b_y) && m >= 0)
{
if ( m <= shortestDistance )
{
shortestDistance = m;
closestWallIndex = i;
}
}
}
float likelihood = exp(-pow(z - shortestDistance, 2) * 1.0 / (2 * pow(0.44, 2))) + 0.1;
return likelihood;
}
void getMotorSpeeds(int &motorSpeedD, int &motorSpeedE, int &motorSpeedF, int &motorSpeedG, int angle, int Vb) {
float Vw1, Vw2, Vw3, Vw4, norm_factor;
Vw1 = Vb*cosDegrees(angle);
Vw2 = Vb*(cosVal[0]*cosDegrees(angle) + sinVal[0]*sinDegrees(angle));
Vw3 = Vb*(cosVal[1]*cosDegrees(angle) + sinVal[1]*sinDegrees(angle));
Vw4 = Vb*(cosVal[2]*cosDegrees(angle) + sinVal[2]*sinDegrees(angle));
norm_factor = 1.0;
if (Vw1 > MAXMOTORSPEED) {
norm_factor = MAXMOTORSPEED / Vw1;
} else if (Vw2 > MAXMOTORSPEED) {
norm_factor = MAXMOTORSPEED / Vw2;
} else if (Vw3 > MAXMOTORSPEED) {
norm_factor = MAXMOTORSPEED / Vw3;
} else if (Vw4 > MAXMOTORSPEED) {
norm_factor = MAXMOTORSPEED / Vw4;
}
motorSpeedD = round(Vw1 * norm_factor);
motorSpeedE = round(Vw2 * norm_factor);
motorSpeedF = round(Vw3 * norm_factor);
motorSpeedG = round(Vw4 * norm_factor);
}
void MoveRobot(int angle, int Vb, int rotSpeed) {
float Vw1, Vw2, Vw3, norm_factor;
// Adjust the angle to make it more intuitive
angle-=90;
// Calculate the individual motor speeds
Vw1 = rotSpeed + Vb * cosDegrees(angle);
Vw2 = rotSpeed + Vb * (-0.6 * cosDegrees(angle) + 0.8 * sinDegrees(angle));
Vw3 = (rotSpeed + Vb * (-0.6 * cosDegrees(angle) - 0.8 * sinDegrees(angle))) * -1;
// This normalises all of the values to make sure
// no motor value peaks over 100%
if (Vw1 > MAXMOTORSPEED) {
norm_factor = MAXMOTORSPEED / Vw1;
} else if (Vw2 > MAXMOTORSPEED) {
norm_factor = MAXMOTORSPEED / Vw2;
} else if (Vw3 > MAXMOTORSPEED) {
norm_factor = MAXMOTORSPEED / Vw3;
} else {
norm_factor = 1.0;
}
// Power the motors.
motor[Thing1] = Vw1 * norm_factor;
motor[Thing2] = Vw2 * norm_factor;
motor[CatintheHat] = Vw3 * norm_factor;
}
// Use some trig to culcalate the required power for each motor
void MoveRobot(int angle, int Vb, int rotSpeed) {
float Vw1, Vw2, Vw3, maxSpeed, norm_factor;
int iVw1, iVw2, iVw3;
// This is where the magic happens. The actual formula is
// Vw = rotSpeed + Vb * ((cosDegrees(wheelAngle) * cosDegrees(movementAngle)) + (sinDegrees(wheelAngle) * sinDegrees(movementAngle)))
// I have optimised the formulae below to reduce the number of operations required to calculate the motor speeds
// since the motorAngle value never changes and cancels some things out, further simplifying the calculations.
// I like simple!
Vw1 = rotSpeed + Vb * cosDegrees(angle);
Vw2 = rotSpeed + Vb * (-0.5 * cosDegrees(angle) + 0.866025 * sinDegrees(angle));
Vw3 = rotSpeed + Vb * (-0.5 * cosDegrees(angle) - 0.866025 * sinDegrees(angle));
// This makes sure the motor values never exceed the maximum (MAXMOTORSPEED)
maxSpeed = max3(abs(Vw1), abs(Vw2), abs(Vw3));
norm_factor = (maxSpeed > MAXMOTORSPEED) ? (MAXMOTORSPEED / maxSpeed) : 1.0;
iVw1 = round(Vw1 * norm_factor);
iVw2 = round(Vw2 * norm_factor);
iVw3 = round(Vw3 * norm_factor);
motor[motorA] = iVw1;
motor[motorB] = iVw2;
motor[motorC] = iVw3;
}
static GRectangle getBoundsGArc(GArc arc) {
double rx, ry, cx, cy, p1x, p1y, p2x, p2y, xMin, xMax, yMin, yMax;
rx = arc->width / 2;
ry = arc->height / 2;
cx = arc->x + rx;
cy = arc->y + ry;
p1x = cx + cosDegrees(arc->u.arcRep.start) * rx;
p1y = cy - sinDegrees(arc->u.arcRep.start) * ry;
p2x = cx + cosDegrees(arc->u.arcRep.start + arc->u.arcRep.sweep) * rx;
p2y = cy - sinDegrees(arc->u.arcRep.start + arc->u.arcRep.sweep) * ry;
xMin = fmin(p1x, p2x);
xMax = fmax(p1x, p2x);
yMin = fmin(p1y, p2y);
yMax = fmax(p1y, p2y);
if (containsAngle(arc, 0)) xMax = cx + rx;
if (containsAngle(arc, 90)) yMin = cy - ry;
if (containsAngle(arc, 180)) xMin = cx - rx;
if (containsAngle(arc, 270)) yMax = cy + ry;
if (arc->filled) {
xMin = fmin(xMin, cx);
yMin = fmin(yMin, cy);
xMax = fmax(xMax, cx);
yMax = fmax(yMax, cy);
}
return createGRectangle(xMin, yMin, xMax - xMin, yMax - yMin);
}
void GObject::Matrix2D::applyRotate(double theta) {
// Counterintuitive sign weirdness
// because positive y-axis points downward.
double m00 = cosDegrees(theta) * m[0][0] - sinDegrees(theta) * m[0][1];
double m01 = sinDegrees(theta) * m[0][0] + cosDegrees(theta) * m[0][1];
double m10 = cosDegrees(theta) * m[1][0] - sinDegrees(theta) * m[1][1];
double m11 = sinDegrees(theta) * m[1][0] + cosDegrees(theta) * m[1][1];
m[0][0] = m00;
m[0][1] = m01;
m[1][0] = m10;
m[1][1] = m11;
}
void runMotorSpeeds(int &motorSpeedD, int &motorSpeedE, int &motorSpeedF, int &motorSpeedG, int angle, int Vb) {
float Vw1, Vw2, Vw3, Vw4, norm_factor;
Vw1 = Vb*cosDegrees(angle);
Vw2 = Vb*sinDegrees(angle);
Vw3 = -Vw1;
Vw4 = -Vw2;
norm_factor = 1.0;
if (Vw1 > MAXMOTORSPEED) {
norm_factor = MAXMOTORSPEED / Vw1;
} else if (Vw2 > MAXMOTORSPEED) {
norm_factor = MAXMOTORSPEED / Vw2;
} else if (Vw3 > MAXMOTORSPEED) {
norm_factor = MAXMOTORSPEED / Vw3;
} else if (Vw4 > MAXMOTORSPEED) {
norm_factor = MAXMOTORSPEED / Vw4;
}
motorSpeedD = roundit(Vw1 * norm_factor);
motorSpeedE = roundit(Vw2 * norm_factor);
motorSpeedF = roundit(Vw3 * norm_factor);
motorSpeedG = roundit(Vw4 * norm_factor);
motor[motorD] = motorSpeedD;
motor[motorE] = motorSpeedE;
motor[motorF] = motorSpeedF;
motor[motorG] = motorSpeedG;
}
void calc_koordinaten_Drive()
{
int preHitpositionX;
int preHitpositionY;
int DegreeTemp = _degree_Hit /10;
if(_degree_Hit >= 0)
{
preHitpositionX = 0;
preHitpositionY = 0;
}
else
{
DegreeTemp = _degree_Hit / -10;
preHitpositionX = (int)(50.0 * (sinDegrees(DegreeTemp)));
preHitpositionY = (int)(50.0 * (cosDegrees(DegreeTemp)));
}
_distance_X2Drive = _distance_X2Ball + preHitpositionX;
_distance_Y2Drive = _distance_Y2Ball - preHitpositionY; //da nicht so weit zum Ball zu fahren ist
nxtDisplayStringAt(0, 8, "DX:%02d DY:%02d ",_distance_X2Drive,_distance_Y2Drive);
}
//----Shifts the activity in the pose structure----//
void pathIntegrateCell(char xp, char yp, char thetap, float deltaTheta, float translation)
{
//initialise loop variables
char relativeX;
char relativeY;
char relativeTheta;
char x;
char y;
char theta;
float deltaPoseX = (cosDegrees(currentDirection) * translation) / 0.5;/// (lengthX / sizeX);
float deltaPoseY = (sinDegrees(currentDirection) * translation) / 0.5; //(lengthX / sizeX);
float deltaPoseTheta = deltaTheta / 60;//(360 / sizeTheta);
int intOffsetX = (int) deltaPoseX; //only a whole number of cells moved
int intOffsetY = (int) deltaPoseY;
int intOffsetTheta = (int) deltaPoseTheta;
getActivationDistribution(deltaPoseX - intOffsetX, deltaPoseY - intOffsetY, deltaPoseTheta - intOffsetTheta);
for(relativeX = 0; relativeX < 2; relativeX++)
{
x = getWrappedX(xp + intOffsetX + relativeX);
for(relativeY = 0; relativeY < 2; relativeY++)
{
y = getWrappedY(yp + intOffsetY + relativeY);
for(relativeTheta = 0; relativeTheta < 2; relativeTheta++)
{
theta = getWrappedTheta(thetap + intOffsetTheta + relativeTheta);
tempPose[x].array2D[y][theta] += distribution[relativeX].array2D[relativeY][relativeTheta] * poseWorld.poseActivity[xp].array2D[yp][thetap];
}
}
}
}
task posTrack() {
static int timeLast,
time,
dt,
thetaLast,
gyroVal,
lEnc,
rEnc;
while(1) {
thetaLast = gyroVal;
timeLast = time;
gyroVal = SensorValue[gyro];
time = nSysTime;
dt = time - timeLast;
lEnc = SensorValue[lEnc];
rEnc = SensorValue[rEnc];
upAvg(&driveVelAvg,
avg(diff(&lDriveDiff, lEnc, dt),
diff(&rDriveDiff, rEnc, dt)
)
);
if((fabs(gyroVal - thetaLast) / dt) > SensorBias[gyro])
theta += (gyroVal - thetaLast);
x += driveVelAvg.mean * dt * sinDegrees(theta);
y += driveVelAvg.mean * dt * cosDegrees(theta);
}
}
int findLine()//Sweeps the servo-mounted light sensor to find a value that is 40 points from the absolute value of the initial reading.
{
servo[servo2] = 0;
int angle = 0;
int initValue = 0;
int raw = 0;
bool isFinished = false;
int i = 127;
LSsetActive(LightSensor);
wait10Msec(100);
initValue = LSvalRaw(LightSensor);
nxtDisplayBigTextLine(2, "%d", initValue);
raw = initValue;
for(i = 0; abs(raw-initValue)<=40 && i < 255; i++)
{
raw = LSvalRaw(LightSensor);
nxtDisplayBigTextLine(4, "%d", raw);
if(abs(raw - initValue)> 4)
{
isFinished = true;
}
wait10Msec(1);
servo[servo2] = i;
}
angle = i * 180/255;
int distance = 40*cosDegrees(angle);
return distance;//The distance tells us how far we need to move the V-Bucket(Trademark Pending) in order to score autonomously.
};
void points_update(float value, int state) {
// Value is distance in cm when state is MOVE_STATE
//and degrees when state is ROTATE_STATE
int i;
float e, f;
switch (state) {
case MOVE_STATE:
for(i=0; i<NUMBER_OF_PARTICLES; i++) {
e = sampleGaussian(0.0, 0.881);
f = sampleGaussian(0.0, 0.881);
xArray[i] = xArray[i] + (value + e) * cosDegrees(thetaArray[i]);
yArray[i] = yArray[i] + (value + e) * sinDegrees(thetaArray[i]);
thetaArray[i] = thetaArray[i] + f;
}
break;
case ROTATE_STATE:
for( i=0; i<NUMBER_OF_PARTICLES; i++) {
e = sampleGaussian(0.0, 0.881);
thetaArray[i] = normalize_angle_value(thetaArray[i] + value + e);
}
break;
}
}
void move(int dist)
{
int distance = abs(dist);
if(dist>0)
{
motor[LFW] = 127;
motor[LRW] = 127;
motor[RFW] = 127;
motor[RRW] = 127;
}
else
{
motor[LFW] = -127;
motor[LRW] = -127;
motor[RFW] = -127;
motor[RRW] = -127;
}
nMotorEncoder[LFW] = 0;
while(abs(nMotorEncoder[LFW])/627.2<distance/(4.0*PI))
{
wait1Msec(10);
}
locX+=cosDegrees(rotation)*distance;
locY+=sinDegrees(rotation)*distance;
freeze();
}
void print_10_points()
{
int i;
for (i=0; i<10; ++i)
writeDebugStream("x:%f y:%f theta:%f cos:%f, sin: %f\n",xArray[i], yArray[i], thetaArray[i],
cosDegrees(thetaArray[i]), sinDegrees(thetaArray[i]));
writeDebugStream("-------------\n");
}
void calc_koordinaten_Ball(int Degree, unsigned int Distance)
{
Degree /= 10;
if(Degree >=0)
{
_distance_Y2Ball = (int)((float)Distance * (cosDegrees(Degree)));
_distance_X2Ball = (int)((float)Distance * (sinDegrees(Degree)));
}
else
{
Degree *= -1;
_distance_Y2Ball = (int)((float)Distance * (cosDegrees(Degree)));
_distance_X2Ball = (int)((float)Distance * (sinDegrees(Degree)));
_distance_X2Ball *= -1;
}
nxtDisplayStringAt(0, 16, "X:%02d Y:%02d ",_distance_X2Ball,_distance_Y2Ball);
}
void Backward40cm(){
motor[rightWheel] = ForwardSpeed*-1;
motor[leftWheel] = ForwardSpeed*-1;
int i;
for ( i = 0 ; i < StraightTime ; i+=timeUnit) {
nxtSetPixel( x , y );
x -= cosDegrees(angle);
y -= sinDegrees(angle);
//wait1Msec(timeUnit);
}
}
void Forward40cm(){
motor[rightWheel] = ForwardSpeed;
motor[leftWheel] = ForwardSpeed;
int i;
for (i = 0; i < StraightTime; i += timeUnit) {
nxtSetPixel( x , y );
x += cosDegrees(angle);
y += sinDegrees(angle);
wait1Msec(timeUnit);
}
}
/**
* This displays an arrow on the screen pointing downwards.
* @param degreesFromDown the number of degrees from down
*/
void displayArrow(int degreesFromDown)
{
eraseDisplay();
// Otherwise, the arrow would point up.
degreesFromDown = degreesFromDown-180;
//If you don't know trigonometry, you can ignore this part
nxtDrawLine(49,
31,
(cosDegrees(degreesFromDown ) * 20) + 49,
(sinDegrees(degreesFromDown ) * 20) + 31);
nxtDrawLine((cosDegrees(degreesFromDown - 20) * 15) + 49,
(sinDegrees(degreesFromDown - 20) * 15) + 31,
(cosDegrees(degreesFromDown ) * 20) + 49,
(sinDegrees(degreesFromDown ) * 20) + 31);
nxtDrawLine((cosDegrees(degreesFromDown + 20) * 15) + 49,
(sinDegrees(degreesFromDown + 20) * 15) + 31,
(cosDegrees(degreesFromDown ) * 20) + 49,
(sinDegrees(degreesFromDown ) * 20) + 31);
}
task updateHUD () {
int x = 0;
int y = 0;
while (true) {
nxtEraseRect(4,50, 44,10);
nxtDisplayTextLine(2, " H: %3d", angleI/100);
nxtDisplayTextLine(3, " X: %3d", x_accel/100);
nxtDisplayTextLine(4, " Y: %3d", y_accel/100);
nxtDisplayTextLine(5, " Z: %3d", z_accel/100);
nxtDrawCircle(84, 50, 4);
nxtDrawCircle(4, 50, 40);
x = (cosDegrees(-1 * (angleI/100 - 90)) * 20) + 24;
y = (sinDegrees(-1 * (angleI/100 - 90)) * 20) + 30;
nxtDrawLine(24, 30, x, y);
nxtEraseRect(0,0, 99, 8);
nxtDrawRect(0,0, 99, 8);
nxtFillRect(50,0, (float)(rotI / 150)/100.0 *50 + 50, 8);
wait1Msec(100);
}
}
void stopAll(){
if(move){
float rot = getCompassValue();
float x = sinDegrees(rot) * encoder;
float y = cosDegrees(rot) * encoder;
nxtDisplayTextLine(0, "%d %d %d", rot, x, y);
nxtDisplayTextLine(1, "%d", encoder);
totalX+=x;
totalY+=y;
nMotorEncoder[motorB] = 0;
move = false;
}
stopBack();
stopF();
stopLeft();
stopRight();
motor[clawm] = 0;
}
void edgeToPoint(Edge & e, Point & p) {
p.x = e.length * sinDegrees(e.angle);
p.y = e.length * cosDegrees(e.angle);
}
void position_add_distance(Position* p, float distance) {
p->x += distance * cosDegrees(p->angle);
p->y += distance * sinDegrees(p->angle);
return;
}
void navigate_to_waypoint(float x, float y)
{
float med_x = 0, med_y = 0, med_theta = 0;
float i, z;
if (DEBUG)
{
writeDebugStream("Going towards point %f, %f\n",x,y);
print_10_points();
print_10_cwa();
}
float cosSum = 0, sinSum = 0;
// estimate current posistion
for (i=0; i < NUMBER_OF_PARTICLES; ++i)
{
med_x += xArray[i]*weightArray[i];
med_y += yArray[i]*weightArray[i];
//med_theta += thetaArray[i] * weightArray[i];
float theta = thetaArray[i];
cosSum += cosDegrees(theta) * weightArray[i];
sinSum += sinDegrees(theta) *weightArray[i];
}
// Transform back from vector to angle
if (cosSum > 0)
med_theta = atan (sinSum / cosSum);
else if (sinSum >=0 && cosSum < 0)
med_theta = atan(sinSum / cosSum) + PI;
else if (sinSum < 0 && cosSum < 0)
med_theta = atan (sinSum / cosSum) - PI;
else if (sinSum > 0 && cosSum == 0)
med_theta = PI;
else if (sinSum < 0 && cosSum == 0)
med_theta = -PI;
else
med_theta = 0;
if (DEBUG)
writeDebugStream("Averages: x: %f y:%f theta:%f\n", med_x, med_y, med_theta);
// calculate difference
float dif_x = x - med_x; // dest - curr_pos
float dif_y = y - med_y;
// Setting the dif_? thresholds
if ( abs(dif_x) < 0.01 )
dif_x = 0.0;
if ( abs(dif_y) < 0.01 )
dif_y = 0.0;
writeDebugStream("Dif_x: %f, dif_y: %f\n",dif_x, dif_y);
float rotate_degs;
// get the nr of degrees we want to turn. But in which direction ?!
// Use of atan2
if ( dif_x > 0)
rotate_degs = atan (dif_y / dif_x);
else if (dif_y >=0 && dif_x < 0)
rotate_degs = atan( dif_y / dif_x) + PI;
else if (dif_y < 0 && dif_x < 0)
rotate_degs = atan (dif_y / dif_x) - PI;
else if (dif_y > 0 && dif_x == 0)
rotate_degs = PI;
else if (dif_y < 0 && dif_x == 0)
rotate_degs = -PI;
else
rotate_degs = 0;
rotate_degs = rotate_degs * 180.0 / PI;
rotate_degs = normalize_angle_value(rotate_degs - med_theta);
writeDebugStream("Rotate angle: %f\n",rotate_degs);
// Print 10 points before rotation
if (DEBUG)
print_10_points();
// Rotate towards the correct position
rotate(rotate_degs);
points_update(rotate_degs, ROTATE_STATE);
// Print 10 points after rotation , to make sure that we rotate correctly
if (DEBUG)
print_10_points();
// move forward
float move_distance = sqrt(dif_x * dif_x + dif_y * dif_y);
if (DEBUG)
writeDebugStream("Moving distance: %f\n", move_distance);
move_forward(MOVE_POWER, move_distance);
// Update the position to the new points
points_update(move_distance, MOVE_STATE);
//wait1Msec(2000);
PlaySound(soundBeepBeep);
// Sonar measurement!
z = SensorValue[sonar];
for ( i = 0; i < NUMBER_OF_PARTICLES; i++ )
{
weightArray[i] = calculate_likelihood(xArray[i], yArray[i], thetaArray[i], z);
}
// Resampling after calculating likelihood
resample();
if (DEBUG)
{
writeDebugStream("After Resampling\n");
print_10_cwa
();
}
}
float tan(float theta)
{
return sinDegrees(theta)/cosDegrees(theta);
}
//===========================================================================================
// Main program - handle joystick inputs and take appropriate actions
//===========================================================================================
task main()
{
int x1, y1;
float hypoteneuse;
bool abortProgram = false;
initializeRobot(); // run all initialisations
waitForStart(); // then wait for FCS to tell us to go
while(!abortProgram) // handle joystick inputs until the drive team aborts program
// drve team can abort program by pressing both "9" buttons at the same time
{ // run first read of joystick to verify connectivity
if(joystick.joy1_TopHat == -1) Joy1Enabled = true; // enable joystick only if tophat responds with -1 // otherwise disable that joystick
if(joystick.joy2_TopHat == -1) Joy2Enabled = true; // same test for joystick 2
if(joy1Btn(10) && joy2Btn(10)) abortProgram = true; // handle the abort program condition
//================================
getJoystickSettings(joystick); // get joystick data packet
x1 = (joystick.joy1_x1*100)/127; // extract X value for joystick one ranged to percentage
y1 = (joystick.joy1_y1*100)/127; // extract Y value for joystick one range to percentage
hypoteneuse=(float)sqrt((x1*x1)+(y1*y1)); // calculate third side of effective triangle
//---------------------
if(joy1Btn(5)) hypoteneuse = hypoteneuse; // max speed only if button 5 is pressed
else if(joy1Btn(7)) hypoteneuse = hypoteneuse/(float)5; // button seven gives slow slow speed
else hypoteneuse = hypoteneuse/(float)2; // otherwise default is half speed
//---------------------thisone
if ((abs(x1)<DEADBAND) && (abs(y1)<DEADBAND)) // if joystick is effectively in zero position
{
drive_speed=0; // then set speed and effective angle to zero
drive_angle=0; //
}
else // joystick is active, so calculate motor power values
{
drive_angle=radiansToDegrees(atan2(x1,y1)); // calculate drive angle from X and Y joystick values
drive_speed=hypoteneuse; // the speed is defined by the extent of the X-Y hypoteneuse
}
//==============================================================
// Code to handle JOG capability using TOPHAT on either joystick
//==============================================================
if(Joy2Enabled == true && joystick.joy2_TopHat!=-1) // if enabled and pressed then we have a request
{
if (!Jog_In_Progress) // only accept new requests if one isn't already active
{
requested_angle = 45*joystick.joy2_TopHat; // calculate and save the requested angle
Jog_In_Progress = true; // setup a jog in progress
ClearTimer(T1); // and start the timer
}
if (Jog_In_Progress) // jog movements take precedence over joystick control
{
if (time1[T1]>JOGTIME) // then the jog is complete
{
Jog_In_Progress = false; // so disable jog control
}
else // the jog is still underway
{
drive_angle = (float)requested_angle; // so the jog angle takes precedence over joystick activity
drive_speed = JOGSPEED; // and we also need to setup the default jog speed
}
}
if (drive_speed>100) drive_speed = 100; // limit max speed to 100 percent
}
else
{
if(Joy1Enabled == true)
{
if ((joystick.joy1_TopHat!=-1) && (!Jog_In_Progress)) // we have a new request for a jog
{
requested_angle = 45*joystick.joy1_TopHat; // calculate and save the requested angle
Jog_In_Progress = true; // setup a jog in progress
ClearTimer(T1); // and start the timer
}
if (Jog_In_Progress) // jog movements take precedence over joystick control
{
if (time1[T1]>JOGTIME) // then the jog is complete
{
Jog_In_Progress = false; // so disable jog control
}
else // the jog is still underway
{
drive_angle = (float)requested_angle; // so the jog angle takes precedence over joystick activity
drive_speed = JOGSPEED; // and we also need to setup the default jog speed
}
}
if (drive_speed>100) drive_speed = 100; // limit max speed to 100 percent
}
}
//================================================================================================
// now calculate individual motor speeds based on the motor angle versus the requested drive angle
//================================================================================================
float gyro_angle=constHeading; // assume we'll be using field relative mode
if(joy1Btn(11) != true) gyro_angle = 0; // otherwise configure robot relative drive
LF_speed = cosDegrees(LF_angle-drive_angle+gyro_angle)*drive_speed; // individual motor speeds are calculated
RF_speed = cosDegrees(RF_angle-drive_angle+gyro_angle)*drive_speed; // relative to the mounted angle of that
LR_speed = cosDegrees(LR_angle-drive_angle+gyro_angle)*drive_speed; // motor, but adjusted for the current
RR_speed = cosDegrees(RR_angle-drive_angle+gyro_angle)*drive_speed; // orientation of the robot per the GYRO
//====================================================================
// Now handle request for rotation, either standalone or while driving
//====================================================================
rotation_factor=0; // assume no rotation is requested
int speed_limit=drive_speed; // this is used to determine maximum allowable speed if there is also rotation
if (joy1Btn(3))
{
if(joy1Btn(5)) rotation_factor=-RotationSpeedFast;// clockwise rotation
else rotation_factor=-RotationSpeed;
}
if (joy1Btn(1))
{
if(joy1Btn(5)) rotation_factor=RotationSpeedFast; // counter clockwise rotation
else rotation_factor=RotationSpeed;
}
speed_limit-=abs(rotation_factor); // adjuist spped limit down to allow for rotation (if any)
//=================================================================================
// now prorate the power levels so the highest equals the requested power level
//=================================================================================
float highest = max(max(abs(LF_speed),abs(RF_speed)),max(abs(LR_speed),abs(RR_speed)));
float adjust_factor=1;
if (highest!=0) adjust_factor=speed_limit/highest;
LF_speed*=adjust_factor;
RF_speed*=adjust_factor;
LR_speed*=adjust_factor;
RR_speed*=adjust_factor;
LF_speed+=rotation_factor; // add in the rotation factor to each drive motor
RF_speed+=rotation_factor;
LR_speed+=rotation_factor;
RR_speed+=rotation_factor;
//=======================================================================
// finally operate the actual drive motors at the calculated power levels
//=======================================================================
motor[LF_motor]=(int)LF_speed;
motor[RF_motor]=(int)RF_speed;
motor[LR_motor]=(int)LR_speed;
motor[RR_motor]=(int)RR_speed;
//=========================================
// ring lifter
//=========================================
if (abs(joystick.joy2_y1)>DEADBAND) // lift activity has been requested by gunner
{
if(joystick.joy2_y1>DEADBAND) // they want it to go UP
{
if(joy2Btn(11)) lifter_state = HIGH_LIFT; // choose between HIGH and LOW levels based on button
else lifter_state=LOW_LIFT;
}
if(joystick.joy2_y1<-DEADBAND) // they want it to go DOWN
{
if(joy2Btn(11)) lifter_state=OVERRIDE_DOWN; // set override mode - ignore limit switches
else lifter_state=MANUAL_DOWN; // otherwise enable down only till limit switch
}
}
else if (abs(joystick.joy1_y2)>DEADBAND) // lift activity has been requested by driver
{
if(joystick.joy1_y2>DEADBAND) // they want it to go UP
{
if(joy1Btn(12)) lifter_state = HIGH_LIFT;
else lifter_state=LOW_LIFT;
}
if(joystick.joy1_y2<-DEADBAND) // they want it to go DOWN
{
if(joy1Btn(12)) lifter_state=OVERRIDE_DOWN;
else lifter_state=MANUAL_DOWN;
}
}
else lifter_state = STOPPED; // if no activity then ensure motor is not running
//=============================
// Gyro reset
//=============================
if(joy1Btn(9)) // this feature allows the drive team to reset the field
{ // relative zero position in the event that there is enough
constHeading = 0; // gyro noise that it gets messed up.
RequestedScreen=S_BIG_NUMS;
}
//=============================
// grabbers
//=============================
if(joy2Btn(4) || joy1Btn(4)) // set UP position (fully closed)
{
if(grabbers_state == grabbersMid || grabbersDown)
{
servoChangeRate[right_servo] = 10;
servoChangeRate[left_servo] = 10;
}
else
{
servoChangeRate[right_servo] = 4;
servoChangeRate[left_servo] = 4;
}
grabbers_state = grabbersUp;
}
if(joy2Btn(3) || joy1Btn(2)) // set MID position (release ring)
{
if(grabbers_state == grabbersDown || grabbers_state == grabbersMid)
{
servoChangeRate[right_servo] = 10;
servoChangeRate[left_servo] = 10;
}
else
{
servoChangeRate[right_servo] = 4;
servoChangeRate[left_servo] = 4;
}
grabbers_state = grabbersMid;
}
if(joy2Btn(2)) // set down position (safety zone)
{
if(grabbers_state == grabbersUp || grabbers_state == grabbersMid)
{
servoChangeRate[right_servo] = 4;
servoChangeRate[left_servo] = 4;
}
else
{
servoChangeRate[right_servo] = 10;
servoChangeRate[left_servo] = 10;
}
grabbers_state = grabbersDown;
}
if(joy2Btn(6) || joy1Btn(6)) // drive team wants grabbers more closed
{
if (!actioned1) // then this is a new request
{
grabbers_state = nudgeUp; // so tell the task to adjust position
actioned1 = true; // and indicate that we handled the request
}
}
else actioned1=false; // once buttons are released then we enable a new request
if(joy2Btn(8) || joy1Btn(8)) // drive team wants grabbers more open
{
if (!actioned2) // then this is a new request
{
grabbers_state = nudgeDown; // so tell the task to adjust position
actioned2=true; // and indicate that we handled the request
}
}
else actioned2 = false; // once the buttons are released we enable a new request
//if(abs(joystick.joy2_y2)>10) motor[lightStrip] = joystick.joy2_y2;
//else motor[lightStrip] = 0;
}
}
float LightSensorToDistanceIn(int count){ //returns distance in inches
int cntdiff = C_SVLIGHTSENSORIN-count;
float degrees = (180.0*cntdiff)/255.0;
float dist = cosDegrees(degrees)*6.5;
return dist;
}
