/** This function tries to recognize the current location.
1. Characterize current location
2. For every learned locations
2.1. Read signature of learned location from file
2.2. Compare signature to signature coming from actual characterization
3. Retain the learned location whose minimum distance with
actual characterization is the smallest.
4. Display the index of the recognized location on the screen */
void recognize_location()
{
loc_sig ls_obs, ls_read;
characterize_location(ls_obs);
float bestScore = -1.0;
float bestScoreAngle = -1.0;
short matchIndex = -1;
float a;
float angle = 0.0;
makeDepthHistogram(0, ls_obs);
for ( short n=0; n < NO_LOCS; n++ )
{
read_signature_from_file(ls_read, n);
makeDepthHistogram(1, ls_read);
a = 0.0;
// COMPARE ls_read with ls_obs and find the best match
for (int i=0; i < NO_BUCKETS; i++)
{
a += pow(depthHistogram_obs[i] - depthHistogram_read[i], 2);
}
writeDebugStreamLine("FILE %d: a=%f", (int) n, a);
if (a < bestScore || bestScore == -1.0) // && a < THRESHOLD)
{
bestScore = a;
matchIndex = n;
}
}
// Location match found, now find robots orientation relative to stored signature
read_signature_from_file(ls_read, matchIndex);
for (int i=0; i < NO_BINS; i++)
{
// Starting at each different bin in the signature calculate a score and then
// use the starting position with the highest score to calculate the robots orientation.
a = 0.0;
for (int j=0; j < NO_BINS; j++)
{
a += (pow((ls_obs.sig[(i+j)%NO_BINS] - ls_read.sig[(i+j)%NO_BINS]), 5));
}
writeDebugStreamLine("ANGLE SCORE: %d %f", i, a);
if (a < bestScoreAngle || bestScoreAngle == -1.0) // && a < THRESHOLD)
{
bestScoreAngle = a;
angle = i*(360/NO_BINS); // Angle equal to index we are at * angle per bin
writeDebugStreamLine("ANGLE SET: %f %f", angle, bestScoreAngle);
}
}
// Display output
nxtDisplayStringAt(10, 30, "MATCH: %d", (int) matchIndex);
nxtDisplayStringAt(10, 20, "SCORE: %f", bestScore);
nxtDisplayStringAt(10, 10, "ANGLE: %f", angle);
}
task main()
{
int tempAngle;
eraseDisplay();
wait1Msec(11);
nxtDisplayStringAt(0, 63, "Minigolf 2013");
nxtDisplayStringAt(0, 48, "GyroSensor:");
nxtDisplayStringAt(0, 40, "SonarSensor:");
//StartTask(GyroDeviceDriver);
StartTask(DistDeviceDriver);
StartTask(getHeading);
while(!_GyrodriverInitFinish) // Gyro initialisierung
{
EndTimeSlice();
}
tempAngle = (int)_fGyroAngle;
wait1Msec(4000);
while(tempAngle != (int)_fGyroAngle)
{
nxtDisplayStringAt(0, 48, "GYRO FAIL, %02d",(int)_fGyroAngle);
nxtDisplayStringAt(0, 40, "Drift after INIT");
}
searchObjekt();
wait1Msec(500);
calc_koordinaten_Ball(_ObjectAngle,_ObjectDistance+45);
wait1Msec(500);
calc_degree_hit();
wait1Msec(500);
calc_koordinaten_Drive();
wait1Msec(500);
turn2Degree(900);
wait1Msec(500);
driveDistance(_distance_X2Drive);
wait1Msec(500);
turn2Degree(0);
wait1Msec(500);
driveDistance(_distance_Y2Drive);
wait1Msec(500);
turn2Degree(_degree_Hit);
wait1Msec(500);
driveDistance(50);
wait1Msec(500);
schlagen(40,60);
while(1)
{
EndTimeSlice();
}
StopAllTasks();
}
void turnTowardsLight()
{
nxtDisplayStringAt(10, 30, "Follow...");
motor[motorC] = K*(lightR-lightL);
motor[motorB] = -K*(lightR-lightL);
postScanDetection = false;
}
//************************
// Segue Linha
//************************
void F_STATE_LINHA()
{
MTASK_SET_RUN(MT_STOP_BUTTON);
int Error = 0;
while(ESTADO_IS_CURRENT())
{
//=====================================
int Error = LLreadAverage(S1)-45;
if(Error ==-45){
Error = 0;
PlayTone(200,1);
}
nxtDisplayStringAt(20, 20, "%i",Error);
motor[motorA]=-20 - Error;
motor[motorC]=-20 + Error;
//Se detectou rampa com acceleracao, sobe a rampa
if(ACCEL_Rampa)ESTADO_SET_TARGET(ST_SOBE);
//=====================================
}
}
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);
}
//----Display Max Activated cell----//
void displayMax()
{
char tempX, tempY, tempTheta;
tempX = (char) poseWorld.maxActivatedCell.x;
tempY = (char) poseWorld.maxActivatedCell.y;
tempTheta = (char) poseWorld.maxActivatedCell.theta;
eraseDisplay();
nxtDisplayTextLine(1, "pose: %2d,%2d", tempX, tempY);
nxtDisplayStringAt(68, 55, ",%2d", tempTheta);
}
void calc_degree_hit(void)
{
int distance_X_ball2hole = _pos_hole_x + _distance_X2Ball;
int distance_Y_ball2hole = _distance_Y2Ball - _pos_hole_y;
_degree_Hit = (int)((atan((float)distance_Y_ball2hole/(float)distance_X_ball2hole))/PI*180.0);
//_degree_Hit *= -10;
nxtDisplayStringAt(50, 32, "AH: %02d",_degree_Hit);
}
task main () {
nNxtButtonTask = -2;
int strength_1;
int strength_2;
int strength_3;
int strength_4;
int strength_5;
nxtDisplayCenteredTextLine(0, "Tris10");
nxtDisplayCenteredBigTextLine(1, "FindBall");
nxtDisplayCenteredTextLine(3, "Test 1");
nxtDisplayCenteredTextLine(5, "Connect sensor");
nxtDisplayCenteredTextLine(6, "to S1");
wait1Msec(2000);
eraseDisplay();
nxtDisplayCenteredTextLine(0, "Tris10 FindBall");
while (true) {
nxtEraseRect(0, 0, 99, 50);
T10FBreadAllStrength(TR10FB, &strength_1, &strength_2, &strength_3, &strength_4, &strength_5);
// Read the current data from the sensor
nxtFillRect( 5, strength_1/4, 10, 0);
nxtFillRect(15, strength_2/4, 20, 0);
nxtFillRect(25, strength_3/4, 30, 0);
nxtFillRect(35, strength_4/4, 40, 0);
nxtFillRect(45, strength_5/4, 50, 0);
nxtDisplayStringAt(52, 40," 1: %3d", strength_1);
nxtDisplayStringAt(52, 32," 2: %3d", strength_2);
nxtDisplayStringAt(52, 24," 3: %3d", strength_3);
nxtDisplayStringAt(52, 16," 4: %3d", strength_4);
nxtDisplayStringAt(52, 8," 5: %3d", strength_5);
wait1Msec(100);
}
}
void doDrawPoint(int top, int x_pos, int y_pos) {
nxtEraseRect(x_pos, 63, x_pos+5, 0);
if (displayUnit == showcurrent)
nxtDisplayCenteredTextLine(7, "Max: %d mA", top);
else if (displayUnit == showvoltage)
nxtDisplayCenteredTextLine(7, "Max: %d mV", top);
nxtDisplayStringAt(0, 12, "0");
nxtDrawLine(10, 10, 10, 60);
nxtDrawLine(10, 10, 15, 10);
nxtDrawLine(10, 35, 15, 35);
nxtDrawLine(10, 60, 15, 60);
nxtSetPixel(x_pos, y_pos);
}
/* This function reads 'threshold' and 'k' from background calibration
* file "libCalib.dat" and wheel calibration file "compassCalib.dat" */
bool readCalib(){
//read the max and min from file "libCalib.dat"
TFileIOResult nIoResult;
short filehandle;
const string filename1 = "libCalib.dat";
short filesize = 1000;
OpenRead(filehandle, nIoResult, filename1, filesize);
short min, max;
ReadShort(filehandle, nIoResult, min);
ReadShort(filehandle, nIoResult, max);
//if file not existed, beep!
if(nIoResult){
nxtDisplayStringAt(0,31,"Please run background calibration program.");
PlayTone(1175,15);
wait1Msec(2000);
return false;
}
Close(filehandle, nIoResult);
threshold = (min + max) / 2; //threshold is a global variable
//read k from file "compassCalib.dat"
const string filename2 = "compassCalib.dat";
OpenRead(filehandle, nIoResult, filename2, filesize);
ReadFloat(filehandle, nIoResult, k); //k is a global variable
//if file not existed, beep!
if(nIoResult){
nxtDisplayStringAt(0,31,"Please run compass calibration program.");
PlayTone(1175,15);
wait1Msec(2000);
return false;
}
Close(filehandle,nIoResult);
return true;
}
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);
}
//************************
// Segue Linha
//************************
void F_STATE_LINHA()
{
MTASK_SET_RUN(MT_STOP_BUTTON,MT_LL,MT_ACCEL);
float Error = 0;
float Integral = 0;
float Out = 0;
MV_StopMotors();
wait10Msec(2);
while(ESTADO_IS_CURRENT())
{
//=====================================
//int Error = LLreadAverage(S1)-45;
wait10Msec(2);
Error = LL_Avr - 45
if(Error ==-45){
Error = 0;
PlayTone(200,10);
ESTADO_SET_TARGET(ST_GAP);
}
nxtDisplayStringAt(20, 20, "%i",Error);
Integral += Error*0.002;
Out = Error*1.2 + Integral;
if(Out<0){
nSyncedMotors = synchCA;
}else{
nSyncedMotors = synchAC;
}
motor[motorA]=50;
motor[motorC]=50;
nSyncedTurnRatio = abs(Out);
//Se detectou rampa com acceleracao, sobe a rampa
if(ACCEL_Rampa)ESTADO_SET_TARGET(ST_SOBE);
//=====================================
}
}
void PIDDriver(short target){
float e, e_dot, old_e = threshold - SensorValue[Light], E = 0;
short u;
time1[T1] = 0;
time1[T2] = 0;
time1[T4] = 0;
float pre_compass = compass();
while(true){
if(time1[T1] < timeout){ // T1 for timeout
e = threshold - SensorValue[Light];
e_dot = e - old_e;
E += e;
u = (short)(KP * e + KD * e_dot + KI * E);
old_e = e;
motor[Left] = cruise_speed - u;
motor[Right] = cruise_speed + u;
if(SensorValue[Light] < target){ //target can be make more conservative than threshold
time1[T1] = 0;
}
if(time1[T2] > sampleFreq && SensorValue[Light] < target){ //the light sensor should be on the tape when it beeps
float cur_compass = compass();
if(time1[T3] > beepInterval && ((cur_compass - pre_compass) > beepThreshold || (pre_compass - cur_compass) > beepThreshold)){
PlayTone(1175, 20);
time1[T3] = 0; // T3 for beepInterval
}
pre_compass = cur_compass;
time1[T2] = 0; // T2 for sampling frequency
float tmp = signedCompass();
nxtDisplayStringAt(0,31,"%f", tmp);
logCompass(tmp);
}
}
else{
if(!turnOnSpot(target)) return;
}
}
}
void jumpTo() //a function to jump to a specific point in our directions
{
while(nNxtButtonPressed != 3) //while the orange button is NOT pressed...
{
bNxtLCDStatusDisplay = true;
if(nNxtButtonPressed == 1) //if the right button is pressed...
{
dirIndex ++; //increse the index variable by 1
}
if(nNxtButtonPressed == 2) // if the left button is pressed...
{
dirIndex --; //decrese the index variable by 1.
}
if(dirIndex < 0) //to keep the index variable from going negative
{ //(no negative array slots)
dirIndex = 0;
}
eraseDisplay();
nxtDisplayRICFile(0, 0, "nKISA.RIC"); // displaying our logo
nxtDisplayBigStringAt(65, 55, "%d", dirIndex);
nxtDisplayStringAt(65, 25, "%d", directions[dirIndex][0]);
wait1Msec(200);
}
}
task main()
{
intDirections();
dirDecode();
StartTask(sensors); //start all our other tasks
StartTask(motors);
StartTask(line);
jumpTo();
bFloatDuringInactiveMotorPWM = false; //make sure the motors brake, not float
// resetMotors = true;
while(true)
{
if(directions[dirIndex][0] == 000)
{
StopAllTasks();
}
eraseDisplay();
nxtDisplayRICFile(0, 0, "nKISA.RIC"); // displaying our logo
nxtDisplayBigStringAt(65, 55, "%d", dirIndex);
nxtDisplayStringAt(65, 25, "%d", directions[dirIndex][0]);
nMotorPIDSpeedCtrl[left] = mtrSpeedReg; //turn off the PID for the motors,
nMotorPIDSpeedCtrl[right] = mtrSpeedReg; //better reaction time
if(dirIndex>1)
{
if(directions[dirIndex-1][1] == 2 && directions[dirIndex][2] == 2) //if our last turn was a left turn AND we
{ //are going to follow the opposite line, move over to that side
turning = true;
motor[left] = 50;
while(nMotorEncoder[left] <= 200) {}
motor[left] = 0;
motor[right] = 50;
while(nMotorEncoder[right] <= 200) {}
motor[right] = 0;
turning = false;
}
if(directions[dirIndex-1][1] == 3 && directions[dirIndex][2] == 1) //same as last, except this is done if
{ //last turn was a right, AND we are
turning = true; //following the left line
motor[right] = 50;
while(nMotorEncoder[right] <= 200) {}
motor[right] = 0;
motor[left] = 50;
while(nMotorEncoder[left] <= 200) {}
motor[left] = 0;
turning = false;
}
}
resetMotors = true;
targetDPS = 450;
lineFollow = true;
wait1Msec(100);
if(directions[dirIndex][1] == 6 || directions[dirIndex][1] == 7) // if parking...
{
for(int i = 0; i < 30; i = i) //if we have seen blue continually for a
{ //certain amount of time,
if(lineColor == 2)
{
i++;
}
else
{
i = 0;
}
writeDebugStreamLine("lineColor is: %d", lineColor);//white to the debug
wait1Msec(10); //stream, better than
} //viewing variables over BT
resetMotors = true;
wait1Msec(20);
while(lastPos < ((parkDistance * (directions[dirIndex][3] - 1)) + 100))
{
wait1Msec(10);
}
lineFollow = false;
targetDPS = 0;
wait1Msec(500);
resetMotors = true;
wait1Msec(500);
turning = true;
switch(directions[dirIndex][1]) // which side of the road are we parking on?
{
case 6:
parkLeft();
break;
case 7:
parkRight();
break;
}
}
else // if not parking, follow the procedure for stopping
{
while(stopSensor != 5) // while the stopSensor is not seeing a stop sign
{
writeDebugStreamLine("stopsensor: %d", stopSensor);
wait1Msec(10); // keep following the line
}
writeDebugStreamLine("!!!STOPPED AT: %d", stopSensor);
PlaySound(soundBeepBeep);
lineFollow = false;
targetDPS = 0;
wait1Msec(500);
resetMotors = true;
wait1Msec(500);
turning = true;
// squareLine(1);
switch(directions[dirIndex][1]) // go to the respective turning functioin
{
case 1: goStraight(); break;
case 2: turnLeft(); break;
case 3: turnRight(); break;
case 4: turnLeftL(); break;
case 5: turnRightL(); break;
}
}
if(directions[dirIndex][1] == 9) //for exiting the city
{
while(lineColor != 5) {}
resetMotors = true;
wait1Msec(20);
while(lastPos < 200)
{
wait1Msec(10);
}
lineFollow = false;
targetDPS = 0;
wait1Msec(500);
resetMotors = true;
wait1Msec(500);
turning = true;
exit();
}
turning = false;
resetMotors = true;
//directions[dirIndex-1][1] = directions[dirIndex][1]; // needed for if we need to move over to the
dirIndex ++; //other side of the road after turn
// if(dirIndex == 14) // for an endless loop on our practice mat
// {
// dirIndex = 0;
// }
//dirDecode();
}
}
void PIDDriver(short target){
float e, e_dot, old_e = threshold - SensorValue[Light], E = 0;
short u;
time1[T1] = 0;
time1[T2] = 0;
time1[T4] = 0;
float pre_compass = compass();
bool greyPatch = false;
while(true){
if(time1[T1] < timeout){ // T1 for timeout
e = threshold - SensorValue[Light];
e_dot = e - old_e;
E += e;
u = (short)(KP * e + KD * e_dot + KI * E);
old_e = e;
motor[Left] = cruise_speed - u;
motor[Right] = cruise_speed + u;
if(SensorValue[Light] < target){ //target can be make more conservative than threshold
time1[T1] = 0;
}
// When grey patch detected
if(SensorValue[Light] > 38 && SensorValue[Light] < 43){
if(time1[T4] > 300){
greyPatch = true;
stop();
// beep three times
PlayTone(1500, 40);
wait1Msec(800);
PlayTone(1500, 40);
wait1Msec(800);
PlayTone(1500, 40);
wait1Msec(800);
control(rotateSpeed, -rotateSpeed, 800); // 800 is for getting rid of the grey patch
while(SensorValue[Light] > threshold){}
stop();
time1[T1] = 0;
time1[T2] = 0;
time1[T3] = 0;
nMotorEncoder[Left]=0;
nMotorEncoder[Right]=0;
pre_compass = compass();
continue;
}
}
else{
time1[T4] = 0;
}
if(time1[T2] > sampleFreq && SensorValue[Light] < target){ //the light sensor should be on the tape when it beeps
float cur_compass = compass();
if(time1[T3] > beepInterval && ((cur_compass - pre_compass) > beepThreshold || (pre_compass - cur_compass) > beepThreshold)){
PlayTone(1175, 20);
time1[T3] = 0; // T3 for beepInterval
}
pre_compass = cur_compass;
time1[T2] = 0; // T2 for sampling frequency
if(greyPatch){ // log the compass value from grey patch to the other endpoint
float tmp = signedCompass();
nxtDisplayStringAt(0,31,"%f", tmp);
logCompass(tmp);
}
}
}
else{
if(greyPatch){
if(!turnOnSpot(target, greyPatch)) return; // reach the endpoint other than grey patch, return
}
else{
turnOnSpot(target, greyPatch);
}
}
}
}
void scanForLight()
{
nxtDisplayStringAt(10, 30, "Scan... ");
rightDeg(2*lastHitDir);
postScanDetection = true;
}
void sumPoseStruct()
{
float tempStruct[10][10];
memset(tempStruct,0,400);
char x,y,z;
float sumActive = 0;
for(x = 0; x<10; x++)
{
for(y=0; y<10; y++)
{
for(z=0; z<6; z++)
{
tempStruct[x][y] += poseWorld.poseActivity[x].array2D[y][z];
}
}
}
sumActive = tempStruct[poseWorld.maxActivatedCell.x][poseWorld.maxActivatedCell.y];
eraseDisplay();
nxtDrawRect(1,1,62,62);
nxtDisplayStringAt(64,60,"x: %2d",poseWorld.maxActivatedCell.x);
nxtDisplayStringAt(64,50,"y: %2d",poseWorld.maxActivatedCell.y);
nxtDisplayStringAt(64,40,"T: %1d",poseWorld.maxActivatedCell.theta);
for(x = 0; x<10; x++)
{
for(y = 0; y<10; y++)
{
char tempPercent = 0;
tempPercent = (tempStruct[x][y] / sumActive)*100;
if(tempPercent<=2) {}
else if(tempPercent>2 && tempPercent<=7)
{
drawRect(x,y,2);
}
else if(tempPercent>7 && tempPercent<=15)
{
drawRect(x,y,15);
}
else if(tempPercent>15 && tempPercent<=25)
{
drawRect(x,y,25);
}
else if(tempPercent>25 && tempPercent<=33)
{
drawRect(x,y,33);
}
else if(tempPercent>33 && tempPercent<=50)
{
drawRect(x,y,50);
}
else if(tempPercent>50 && tempPercent<=66)
{
drawRect(x,y,66);
}
else if(tempPercent>66 && tempPercent<=75)
{
drawRect(x,y,75);
}
else if(tempPercent>75 && tempPercent<=97)
{
drawRect(x,y,85);
}
else if(tempPercent>97 && tempPercent<=100)
{
drawRect(x,y,100);
}
}
}
}
//----Checks if there is a match, add activities, else creates a new association----//
void checkLocalCell()
{
//uses a normalised version of the localTemp. The comparison local cells are stored as normalised when 1st created
char z;
char breakSymbol =0;
signed char cellCount = 0;
char match = 0;
float dotTempValue = 0;
float tempAngle = 0;
if(nextEmptyCell == 0)
{
//first localCellView
writeLocal();
nextEmptyCell++;
PlaySound(soundFastUpwardTones);
while(bSoundActive) {}
}
else {
OpenRead(hFileHandle3,nIoResultRead,sFileName1,nFileSize);
while(nIoResultRead != ioRsltEndOfFile)
{
//search for a previous local cell that matches the current view to a certain degree
for(z = 0; z<numNeuralUnits; z++)
{
ReadFloat(hFileHandle3,nIoResultRead,localComparison.localArray[z]);
if(nIoResultRead == ioRsltEndOfFile) {breakSymbol = 1;break;}
}
if(breakSymbol == 1) {break;}
ReadByte(hFileHandle3, nIoResultRead, injectX);
ReadByte(hFileHandle3, nIoResultRead, injectY);
ReadByte(hFileHandle3, nIoResultRead, injectTheta);
dotTempValue = dotMultiply();
tempAngle = acos(dotTempValue);
if(tempAngle<0.17)//0.17 //if difference less than 10 degrees between vectors
{
match = 1;
Close(hFileHandle3,nIoResultRead);
break; //save cycles break out
}
cellCount++;
}
if(match == 0)
{//no match found - create a new local view cell
Close(hFileHandle3,nIoResultRead);
swapFiles();
nextEmptyCell++;
PlaySound(soundFastUpwardTones);
while(bSoundActive) {}
}
else if(match == 1)
{
injectEnergy(stepSize, injectX, injectY, injectTheta);
nxtDisplayStringAt(64,30,"x: %2d",injectX);
nxtDisplayStringAt(64,20,"y: %2d",injectY);
nxtDisplayStringAt(64,10,"T: %1d",injectTheta);
PlaySound(soundBeepBeep);
while(bSoundActive) {}
}
}
}
