void position_add_angle(Position* p, float deltaAngle) {
if (DEBUG)
writeDebugStream("Position_add_angle: Pos angle:%f adding angle:%f\n",p->angle, deltaAngle);
p->angle = normalize_angle_value(p->angle + deltaAngle);
if (DEBUG)
writeDebugStream("Position_add_angle: Pos angle:%f\n",p->angle);
}
// Y is our IR input, X is our dependant variable.
task main()
{
clearDebugStream();
while(true)
{
sum_y = 0; //from _x->_y
for (int i = 0; i<20 ; i++)
{
HTIRS2readAllACStrength(IR, acS1, acS2, acS3, acS4, acS5);
irInput[i] = acS3; //READINGS: Fill our array with data from the middle node
sum_y += irInput[i]; //CALCULATE: The sum of each value in the array. (its easiest this way)
sum_xTimesy += xInput[i] * irInput[i] ;//EQUATION: Top left half
Sleep(50);
}//APROOVED
mean_xTimesy = (sum_x * sum_y)/20; //EQUATION: Top right half
slopeLOBF = (sum_xTimesy-mean_xTimesy)/(sum_xSquared - mean_sum_xSquared); //CALCULATE: LOBF slope
//y_intLOBF = (sum_y/10) - (5.5 * slopeLOBF); //CALCULATE: LOBF y-intercept -> 5.5 = mean_x
/*
while (y_intLOBF >= y_intTH) //So long as we are above our y-intercept threshold AKA know we are close to the top of the curve
{
if (slopeLOBF <= slopeTH || slopeTH >=) //Wait untill our LOBF slope falls within our calibrated threshold
{
//Make beeping noise perhaps turn left because we have determined the IRSeekerV2 to be facing the IRBeacon
}
}
*/
writeDebugStream("%i, ", slopeLOBF);
writeDebugStream("%i,", y_intLOBF);
writeDebugStreamLine("%i,", numLoop);
numLoop++;
Sleep(50);
}
}
/* L2Distance: computes the L2 (euclidean) distance between st and end.
*/
float L2Distance(node *st, node *end){
if (st == NULL || end == NULL){
writeDebugStream("null point passed to L2Distance! \n");
return 1000000.0; //don't go this way, something's wrong
}
writeDebugStream("endx: %f2 stx: %f2 endy: %f2 sty: %f2\n",end->x,st->x,end->y,st->y);
return sqrt(pow((end->x)-(st->x),2.0)+pow((end->y)-(st->y),2.0));
}
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");
}
// Print the last 10 cumulative weight arrays
void print_10_cwa()
{
writeDebugStream("Printing weights...\n");
int i;
for (i=NUMBER_OF_PARTICLES-11; i<NUMBER_OF_PARTICLES; ++i)
writeDebugStream("wa[%d]: %f - cwa[%d]: %f\n",i,weightArray[i], i,cumulativeWeightArray[i]);
writeDebugStream("-------------\n");
}
int CountBits(byte Counting){
int Ret = 0;
for(int k = 0; k < 8; k++){
if(((0b10000000 >> k )& Counting)>0){
Ret++;
writeDebugStream("1");
}else{
writeDebugStream("0");
}
}
task main()
{
// Initiate BNS Library
BNS();
// Create a 3x3 matrix of zeros
Matrix mat1;
CreateZerosMatrix(&mat1, 3, 3);
// Create a 3x3 matrix with some data in it
// Set location 1 down, 0 across, to be 16
Matrix mat2;
CreateMatrix(&mat2, "1.10 3.40 0; 5 3 2; 0 1 1.234");
SetMatrixAt(&mat2, 1, 0, 16);
// Creates a 3x3 identity matrix, then multiply it by 11
Matrix mat3;
CreateIdentityMatrix(&mat3, 3);
MatrixMultiplyScalar(&mat3, 11);
// Print matricies to the debugger console
PrintMatrix(&mat1);
PrintMatrix(&mat2);
PrintMatrix(&mat3);
// Matrix Examples:
// Matrix determinant
float det = MatrixDeterminant(&mat2);
writeDebugStreamLine("Matrix Det = %f", det);
// Matrix Inverse
Matrix inv;
MatrixInv(&inv, mat2);
writeDebugStream("Inverse ");
PrintMatrix(&inv);
// Matrix Multiplication
Matrix mult;
MatrixMult(&mult, mat2, mat3);
writeDebugStream("Multiply ");
PrintMatrix(mult);
// Matrix Addition
Matrix add;
MatrixAdd(&add, mat2, mat3);
writeDebugStream("Add ");
PrintMatrix(&add);
// Matrix Subtraction
Matrix sub;
MatrixSub(&sub, mat3, mat2);
writeDebugStream("Subtract ");
PrintMatrix(&sub);
}
task main()
{
initReadMode("log.txt", 1024);
for(int i = 0; i < 1000; i++) {
writeDebugStream("%i\t", nextInt());
writeDebugStream("%f\n", (((nextInt())/1000.0)));
nxtDisplayBigTextLine(1, "%i", nextInt());
nxtDisplayBigTextLine(3, "%f", (((nextInt())/1000.0)));
wait1Msec(40);
}
closeActiveFile();
}
//Retrieve data from the sound sensor
void i2c_read_registers_text(ubyte register_2_read, int message_size, int return_size)
{
memset(I2Creply, 0, sizeof(I2Creply));
message_size = message_size+3;
I2Cmessage[0] = message_size; // Messsage Size
I2Cmessage[1] = ARDUINO_ADDRESS;
I2Cmessage[2] = register_2_read; // Register
sendI2CMsg(S1, &I2Cmessage[0], return_size);
wait1Msec(20);
readI2CReply(ARDUINO_PORT, &I2Creply[0], return_size);
int i = 0,top=0;
//Pitch
if(cmd==1)
writeDebugStreamLine("%d", (int)I2Creply[0]+(int)I2Creply[1]*256);
//Sound Power Level
else if(cmd==2)
//For detecting clap
//if((int)I2Creply>85)
writeDebugStreamLine("%i", (int)I2Creply[0]);
//FFT
else if(cmd==3)
{
for(int x = 0; x <return_size; x++)
{
writeDebugStream("%i", I2Creply[x]);
writeDebugStream(" ");
}
writeDebugStreamLine(" ");
//Graphic Equalizer
nxtEraseRect(6,62,99,6);
nxtDrawLine(5,2,5,61);
nxtDrawLine(2,5,95,5);
for(int j=0;j<return_size;j++)
{
top=I2Creply[j]+5;
if(top>61)
top=61;
nxtDrawRect(5+6*j,top,5+6*(j+1),5);
}
}
}
task main()
{
char *ssid = "YOURSSID";
char *wpa_psk = "YOURPASSWORD";
writeDebugStreamLine("ssid: %s", ssid);
writeDebugStreamLine("psk: %s", wpa_psk);
short len;
eraseDisplay();
bNxtLCDStatusDisplay = true; // Enable top status line display
writeDebugStream("Scanning for wifi sensor: ");
// You can play with these to see if they work for you.
// I tend to use the 460800 rate as it's the fastest speed
// that I can use reliably.
//DWIFIsetBAUDRate(9600);
DWIFIsetBAUDRate(230400);
DWIFIresetConfig();
playSound(soundBlip);
while(nNxtButtonPressed != kEnterButton) sleep(1);
DWIFIsetBAUDRate(230400);
configureWiFi((char *)ssid, (char *)wpa_psk);
// set_verbose(false);
// Receive();
sleep(100);
while(true)
{
RS485read(RS485rxbuffer, len, 100);
}
playSound(soundBeepBeep);
}
task main(){
nxtDisplayCenteredTextLine(0, "Dexter Ind.");
nxtDisplayCenteredBigTextLine(1, "IMU");
nxtDisplayCenteredTextLine(3, "Test 1");
nxtDisplayCenteredTextLine(5, "Connect sensor");
nxtDisplayCenteredTextLine(6, "to S1");
wait1Msec(2000);
eraseDisplay();
// Fire up the gyro and initialize it. Only needs to be done once.
//DIMUconfigGyro(DIMU, DIMU_GYRO_RANGE_500);
if (!DIMUconfigAccel(DIMU, DIMU_ACC_RANGE_2G))
PlaySound(soundException);
if(!DIMUconfigGyro(DIMU, DIMU_GYRO_RANGE_250, true))
PlaySound(soundException);
for (int i = 0; i < 500; i++){
// Read the GYROSCOPE
// There are 3 ways to do this:
// All at once, very convenient if you need all 3
DIMUreadGyroAxes(DIMU, xvals[i], yvals[i], zvals[i]);
wait1Msec(5);
}
for (int i = 0; i< 500; i++) {
writeDebugStream("%f, %f", xvals[i], yvals[i]);
writeDebugStreamLine(", %f", zvals[i]);
wait1Msec(2);
}
}
/* L1Distance: computes the L1 (manhattan) distance between st and end.
*/
float L1Distance(node st, node end){
if (st == NULL || end == NULL){
writeDebugStream("null point passed to L1Distance! \n");
return 1000000.0; //don't go this way, something's wrong
}
return abs((end.x)-(st.x))+abs((end.y)-(st.y));
}
task main()
{
eraseDisplay();
T("pie2");
//Calibrate();
Delete(sFileName, nIoResult);
nFileSize = 21 * 10000; // room for 10,000 entries
OpenWrite( hFileHandle, nIoResult, sFileName, nFileSize);
if (nIoResult != ioRsltSuccess)
{
T("Open failed");
writeDebugStream("nAvailFlash: %d", nAvailFlash);
StopAllTasks();
}
PressBumperToContinue();
FollowSegment();
motor[LEFT] = 0;
motor[RIGHT] = 0;
LLsleep(LINELEADER);
Close(hFileHandle, nIoResult);
PlaySound(soundFastUpwardTones);
wait1Msec(2000);
}
void BNS_ERROR(char* errorTitle, char* error)
{
writeDebugStream("***\nBNS ");
writeDebugStreamLine(errorTitle);
writeDebugStreamLine(error);
writeDebugStreamLine("***");
}
task main()
{
initialize();
writeDebugStream("This is chute forward\n");
joyWaitForStart();
time1[T1] = 0; //in ms
driveSetMecMotorS(&desiredMotorVals, 1.0);
while (time1[T1] < 2500) {
motorSetActualPowerToDesired(&desiredMotorVals);
writeDebugStream("Driving forward!\n");
}
driveZeroMecMotor(&desiredMotorVals);
while (time1[T1] < 2500) {
motorSetActualPowerToDesired(&desiredMotorVals);
writeDebugStream("Stopping!\n");
}
}
task main()
{
HTIRS2setDSPMode(irsensor, DSP_1200);
HTIRS2setDSPMode(irsensor2, DSP_1200);
while(true) {
int irArray[5];
int irArray2[5];
HTIRS2readAllACStrength(irsensor, irArray[0], irArray[1], irArray[2], irArray[3], irArray[4]);
HTIRS2readAllACStrength(irsensor2, irArray2[0], irArray2[1], irArray2[2], irArray2[3], irArray2[4]);
int p = (irArray[1]-irArray[2])-10;
writeDebugStream("(%d) ",p);
for (int i = p+20; i > 0;i-=2){
writeDebugStream("#");
}
}
}
void initialize() {
clearDebugStream();
writeDebugStream("This is JoyRecord\n");
//Initialize to zeroes
memset(&desiredMotorVals, 0, sizeof(desiredMotorVals));
memset(&desiredEncVals, 0, sizeof(desiredEncVals));
motorInit(&desiredEncVals);
servoInit();
}
task main()
{
initializeRobot();
waitForStart(); // Wait for the beginning of autonomous phase.
int iFrameCnt=0;
//--------------------INIT Code---------------------------//
ForwardDistReset((tMotor)rtWheelMotor, (tMotor)ltWheelMotor);
DirectionReset();
nMotorEncoder[blockthrower] = 0;
speedCmdZ1=0;
pathIdx=0;
delayatruecount=0;
//--------------------End INIT Code--------------------------//
StartTask(Foreground, 255);
while(true){
//-----------------Start i_debug.c output------------------//
if (debugRun==true){// Do not send out a stream if Debug is not called
if (debugSkip<debugSkipI++){
writeDebugStream("[%d] ",iFrameCnt);
for (int i=0; i<debugI; ++i)
writeDebugStream(" %5s=%5d",debugStr[i],debugInt[i]);
writeDebugStream("\n");
debugSkipI=0;
}
iFrameCnt++;
debugRun=false;
debugI=0; //Clear the debug buffer
}
//------------------Stop i_debug.c output----------------------//
}// While
}// Main
task recordToDebugStreamTask()
{
unsigned byte i;
clearTimer(T4);
//Format is <(m)otor><port><value><(t)ime>
while (true)
{
//Print motors
for (i = 0; i < kNumbOfTotalMotors; i++)
{
writeDebugStream("m%d%03d", i + 1, motor[i]);
}
//Print time
writeDebugStream("t%.4f", time1[T4]);
}
}
// Print a Matrix to the console
void PrintMatrix(struct Matrix *A)
{
int i, j;
if(A->array.inUse == true)
{
writeDebugStream("Matrix: %d\n", A->array.pointer);
for(i = 0; i < A->m; i++)
{
for(j = 0; j < A->n; j++)
{
writeDebugStream("%f\t", GetMatrixAt(A, i, j));
}
writeDebugStream("\n");
}
}
else
{
BNS_ERROR("MATRIX ERROR", "Cannot Print an unset or NULL matrix!");
}
}
task main()
{
clearDebugStream();
writeDebugStreamLine("This is PIDTest\n");
memset(&desiredEncVals, 0, sizeof(desiredEncVals));
motorInit(&desiredEncVals);
semaphoreInitialize(PIDconstantSemaphore);
startTask(PID);
long loopStartTimeMs = nPgmTime;
semaphoreLock(PIDconstantSemaphore);
for (pid_kp=0; pid_kp<5; pid_kp++) {
for (pid_ki=0; pid_ki<0; pid_ki+=0.01) {
for (pid_kd=0; pid_kd<0; pid_kd++) {
writeDebugStream("%f, %f, %f, ", pid_kp, pid_ki, pid_kd);
if (bDoesTaskOwnSemaphore(PIDconstantSemaphore)) {
semaphoreUnlock(PIDconstantSemaphore);
}
while(motorGetEncoder((tMotor) MecMotor_FR) < 5000){
hogCPU();
motorUpdateState();
desiredMotorVals.power[MecMotor_FR] = 50;
releaseCPU();
}
semaphoreLock(PIDconstantSemaphore);
motor[MecMotor_FR] = 0; //can do this because we have lock on semaphore
writeDebugStream("Changing constants!\n");
wait1Msec(1000); //wait for motor to spin down
}
}
}
}
void
RCFS_DebugFile( flash_file *f )
{
char str[20];
int i;
// some problem using sprintf here
for(i=0;i<16;i++)
{
if( f->name[i] > 0x20 && f->name[i] < 0x7f )
str[i] = f->name[i];
else
str[i] = ' ';
}
str[16] = 0;
writeDebugStream(str);
writeDebugStream(" Addr %08X", f->addr );
writeDebugStream(" Data %08X", (unsigned long)f->data );
writeDebugStream(" Size %5d", f->datalength );
writeDebugStream(" Type %02X", f->type );
writeDebugStream(" Time %02X%02X%02X%02X", f->time[0],f->time[1],f->time[2],f->time[3] );
// writeDebugStream(" Flag %02X", f->unknown );
writeDebugStreamLine("");
}
void parseInput()
{
writeDebugStreamLine("Beging parsing...");
ubyte BytesRead[20];
ubyte currByte[] = {0};
ubyte prevByte[] = {0};
ubyte conn[] = {0};
int cid;
string tmpString;
int index = 0;
while (true)
{
alive();
if (nxtGetAvailHSBytes() > 0)
{
nxtReadRawHS(currByte[0], 1);
if ((prevByte[0] == 27) && (currByte[0] == 'S')) {
index = 0;
memset(rxbuffer, 0, sizeof(rxbuffer));
wait1Msec(1);
nxtReadRawHS(conn[0], 1);
cid = conn[0] - 48;
writeDebugStreamLine("Conn: %d", cid);
while (true) {
while (nxtGetAvailHSBytes() == 0) EndTimeSlice();
nxtReadRawHS(currByte[0], 1);
if ((prevByte[0] == 27) && (currByte[0] == 'E')) {
rxbuffer[index--] = 0;
rxbuffer[index--] = 0;
PlaySound(soundShortBlip);
while(bSoundActive) EndTimeSlice();
break;
}
prevByte[0] = currByte[0];
rxbuffer[index++] = currByte[0];
}
for (int i = 0; i < ((index / 19) + 1); i++) {
memset(BytesRead[0], 0, 20);
memcpy(BytesRead[0], rxbuffer[i*19], 19);
StringFromChars(tmpString, BytesRead);
writeDebugStream(tmpString);
}
genResponse(cid);
}
prevByte[0] = currByte[0];
}
}
}
task main()
{
initialize();
writeDebugStream("This is slides forward\n");
joyWaitForStart();
wait1Msec(5000);
time1[T1] = 0; //in ms
driveSetMecMotorN(&desiredMotorVals, 1.0);
while (time1[T1] < 2500) {
motorSetActualPowerToDesired(&desiredMotorVals);
writeDebugStream("Driving forward!\n");
}
driveZeroMecMotor(&desiredMotorVals);
while (time1[T1] < 10000) {
motorSetActualPowerToDesired(&desiredMotorVals);
writeDebugStream("Stopping!\n");
}
//lower harvester
while (time1[T1] < 3000) {
servoSetCont(HarvesterWinch, 0);
}
}
void normalise_weight_array ()
{
if (DEBUG)
writeDebugStream("In normalise weight array\n");
//print_10_cwa ();
int i;
float sum = 0.0;
for (i = 0; i < NUMBER_OF_PARTICLES; ++i)
sum += weightArray[i];
if (DEBUG)
writeDebugStream("Sum: %f\n",sum);
for (i = 0; i < NUMBER_OF_PARTICLES; ++i)
weightArray[i] /= sum;
//print_10_cwa ();
if (DEBUG)
writeDebugStream("End normalise weight array\n");
}
void
P3DebugPacket( p3pak *packet )
{
unsigned char *p;
int i;
p = &packet->command.data[0];
for(i=0;i<packet->cmd_len;i++)
writeDebugStream("%02X ",*p++);
writeDebugStreamLine("");
return;
}
void CenterPosition1() {
turnUltra(0, 90);
moveDistanceRamp(50, 12);
binaryTillSenseHeading(120, 270, 30, frontUS);
PlaySound(soundLowBuzzShort);
translateDistance(200, 270, 7);
writeDebugStream("DONE WITH FIRST");
turnUltra(0,0);
pause(0.2);
irTillSensePeak(30);
playSound(soundLowBuzz);
turnWithGyro(30,0);
}
task main()
{
while(true)
{
//update IR
IR_Update();
writeDebugStream("Left::: ");
writeDebugStream("A: %i, ", IR_RightValue.A);
writeDebugStream("B: %i, ", IR_RightValue.B);
writeDebugStream("C: %i, ", IR_RightValue.C);
writeDebugStream("D: %i, ", IR_RightValue.D);
writeDebugStreamLine("E: %i", IR_RightValue.E);
writeDebugStream("Right::: ");
writeDebugStream("A: %i, ", IR_LeftValue.A);
writeDebugStream("B: %i, ", IR_LeftValue.B);
writeDebugStream("C: %i, ", IR_LeftValue.C);
writeDebugStream("D: %i, ", IR_LeftValue.D);
writeDebugStreamLine("E: %i", IR_LeftValue.E);
writeDebugStreamLine("Total: %i", IR_LeftValue.C + IR_RightValue.C);
}
}
/**
* Dump the array contents on the NXT's screen
* @param data the array that is to be displayed
* @param size the amount of data that should be displayed
*/
void dumpRXData(tBigByteArray &data, ubyte size)
{
string tmpBuff;
int bytesleft = size;
int datalen = 0;
for (int i = 0; i < ((size/18) + 1); i++)
{
datalen = (bytesleft > 18) ? 18 : bytesleft;
memset(tmpBuff, 0, 20);
memcpy(tmpBuff, &data[i*18], datalen);
writeDebugStream("BUFFER: ");
writeDebugStreamLine(tmpBuff);
nxtDisplayTextLine(i+4, tmpBuff);
bytesleft -= 18;
}
}
task main()
{
long rate = 0;
eraseDisplay();
bNxtLCDStatusDisplay = true; // Enable top status line display
writeDebugStream("Scanning for wifi sensor: ");
rate = scanBaudRate();
writeDebugStreamLine("%d baud", rate);
configureWiFi();
set_verbose(false);
Receive();
wait1Msec(100);
closeAllConns();
wait1Msec(1000);
clear_read_buffer();
startListen(80);
clear_read_buffer();
parseInput();
}