/*
Main task
*/
task main () {
blob_array _blobs;
// combine all colliding blobs into one
bool _condensed = true;
//blob_array _blobs;
int _l, _t, _r, _b;
int _nblobs;
eraseDisplay();
// Initialise the camera
NXTCAMinit(cam);
while(true) {
eraseDisplay();
// Fetch all the blobs, have the driver combine all
// the colliding blobs.
_nblobs = NXTCAMgetBlobs(cam, _blobs, _condensed);
for (int i = 0; i < _nblobs; i++) {
// Draw the scaled blobs
_l = xscale(_blobs[i].x1);
_t = yscale(_blobs[i].y1);
_r = xscale(_blobs[i].x2);
_b = yscale(_blobs[i].y2);
nxtFillRect(_l, _t, _r, _b);
}
nxtDisplayTextLine(1, "%d", _nblobs);
wait1Msec(100);
}
}
/*
Main task
*/
task main () {
blob_array _blobs;
memset(_blobs, 0, sizeof(blob_array));
// combine all colliding blobs into one
bool _condensed = true;
//blob_array _blobs;
int _l, _t, _r, _b;
int _x, _y;
int _nblobs;
eraseDisplay();
// Initialise the camera
NXTCAMinit(cam);
while(true) {
eraseDisplay();
// Fetch all the blobs, have the driver combine all
// the colliding blobs.
_nblobs = NXTCAMgetBlobs(cam, _blobs, _condensed);
for (int i = 0; i < _nblobs; i++) {
// Draw the scaled blobs
_l = xscale(_blobs[i].x1);
_t = yscale(_blobs[i].y1);
_r = xscale(_blobs[i].x2);
_b = yscale(_blobs[i].y2);
nxtDrawRect(_l, _t, _r, _b);
}
NXTCAMgetAverageCenter(_blobs, 3, 0, _x, _y);
_x = xscale(_x);
_y = yscale(_y);
nxtEraseRect(_x-4, _y-4, _x+4, _y+4);
nxtDrawLine(_x, _y+2, _x, _y-2);
nxtDrawLine(_x+2, _y, _x-2, _y);
nxtDisplayTextLine(1, "%d", _nblobs);
wait1Msec(100);
}
}
/*
Main task
*/
task main () {
blob_array _blobs;
memset(_blobs, 0, sizeof(blob_array));
//blob_array _blobs;
int _x, _y, _x1, _y1;
int _nblobs;
eraseDisplay();
// pid variables
float pwr_x = 0.0;
float err_x = 0.0;
float perr_x = 0.0;
float aerr_x = 0.0;
float sp_x = 0.0;
float p_x = 0.0;
float i_x = 0.0;
float d_x = 0.0;
float pwr_y = 0.0;
float err_y = 0.0;
float perr_y = 0.0;
float aerr_y = 0.0;
float sp_y = 0.0;
float p_y = 0.0;
float i_y = 0.0;
float d_y = 0.0;
sp_y = sp_x = 87;
p_y = p_x = 0.5;
d_y = d_x = 1.8;
i_y = i_x = 0.05;
nMotorEncoder[motorA] = 0;
nMotorEncoder[motorB] = 0;
// Initialise the camera
NXTCAMinit(cam);
while(true) {
eraseDisplay();
// Fetch all the blobs, have the driver combine all
// the colliding blobs.
_nblobs = NXTCAMgetBlobs(cam, _blobs);
nxtDisplayTextLine(1, "%d, %f", _nblobs, perr_x);
if (_nblobs == 0) {
if ((perr_x > 0) && (nMotorEncoder[motorB] > 660)) {
motor[motorA] = 0;
motor[motorB] = 0;
} else if ((perr_x < 0) && (nMotorEncoder[motorB] < -660)) {
motor[motorA] = 0;
motor[motorB] = 0;
}
} else {
_x = (_blobs[0].x2 + _blobs[0].x1)/2;
_y = (_blobs[0].y2 + _blobs[0].y1)/2;
err_x = _x - sp_x;
aerr_x += err_x;
pwr_x = (err_x * p_x) + ((err_x - perr_x) * d_x) + (aerr_x * i_x);
err_y = _y - sp_y;
aerr_y += err_y;
pwr_y = (err_y * p_y) + ((err_y - perr_y) * d_y) + (aerr_y * i_y);
motor[motorA] = round(pwr_y);
motor[motorB] = round(pwr_x);
_x1 = xscale(_x);
_y1 = yscale(_y);
nxtEraseRect(_x1-6, _y1-6, _x1+6, _y1+6);
nxtDrawLine(_x1, _y1+3, _x1, _y1-3);
nxtDrawLine(_x1+3, _y1, _x1-3, _y1);
perr_y = err_y;
perr_x = err_x;
}
wait1Msec(50);
}
}
void readMultipleDataMsgs()
{
TFileIOResult nBTCmdRdErrorStatus;
int nSizeOfMessage;
ubyte nRcvBuffer[kMaxSizeOfMessage];
while (true)
{
// Check to see if a message is available
nSizeOfMessage = cCmdMessageGetSize(kQueueID);
if (nSizeOfMessage <= 0)
{
wait1Msec(1); // Give other tasks a chance to run
break; // No more message this time
}
if (nSizeOfMessage > kMaxSizeOfMessage)
nSizeOfMessage = kMaxSizeOfMessage;
nBTCmdRdErrorStatus = cCmdMessageRead(nRcvBuffer, nSizeOfMessage, kQueueID);
cmps = 180+(nRcvBuffer[0]-nRcvBuffer[1]);
int oxmps;
if(nRcvBuffer[1] == 0){oxmps = nRcvBuffer[0];}
if(nRcvBuffer[0] == 0){oxmps = -nRcvBuffer[1];}
nxtDisplayTextLine(3, "button: %4d", nRcvBuffer[2]);
nxtDisplayTextLine(4, "Abs: %4d", oxmps);
nxtDisplayTextLine(5, "curr: %4d", HTMCreadRelativeHeading(HTMC));
if(nRcvBuffer[2]==1){
int mspeed = 0;
int mangle = 0;
//stopMotors();
blob_array _blobs;
// combine all colliding blobs into one
bool _condensed = true;
//blob_array _blobs;
int _l, _t, _r, _b;
int _nblobs;
eraseDisplay();
// Initialise the camera
NXTCAMinit(cam);
servo[servo1] = 35;
while(true) {
eraseDisplay();
_nblobs = NXTCAMgetBlobs(cam, _blobs, _condensed);
// Fetch all the blobs, have the driver combine all
// the colliding blobs.
if(_nblobs>0){
int cx = 176-(SIDE_CENTER(_blobs[0].x1, _blobs[0].x2));
int cy = SIDE_CENTER(_blobs[0].y1, _blobs[0].y2);
float dx = cx-88;
/* if(cy < C_STOP){
mspeed = (cy/C_STOP)*25;
}
else{
mspeed = 0;
}*/
mspeed=50;
// if((cx-44)>0){
float rathura = (dx*90)/88;
mangle = rathura;
// }
// else{
//mangle = (dx/44)*-90;
//}
// Draw the scaled blobs
_l = xscale(_blobs[0].x1);
_t = yscale(_blobs[0].y1);
_r = xscale(_blobs[0].x2);
_b = yscale(_blobs[0].y2);
nxtFillRect(_l, _t, _r, _b);
nxtDisplayTextLine(1, "%d", _nblobs);
nxtDisplayTextLine(2, "%d", mangle);
nxtDisplayTextLine(3, "%d", (int)dx);
runMotorSpeeds(mSD, mSE, mSF, mSG, mangle, mspeed);
_nblobs = 0;
wait1Msec(100);
}
}
flagged = true;
}
// else{
// stopMotors();
else{
if (abs(oxmps-HTMCreadRelativeHeading(HTMC)) > 5)
{
flagged = false;
}
}
if(flagged == true){
stopMotors();
}
else{
turntocmps(cmps, oxmps);
}
// }
//}
}
return;
}
