void Rover::interpretJoypadButton(int id, bool status){
if(status == true){
for(int i=0; i<numberOfSpeedButtons; i ++){
if(id == speedButtonsIDTable[i]){
setLeftPWM(speedButtonsSpeedsTable[i]);
setRightPWM(speedButtonsSpeedsTable[i]);
}
}
}
else{
setLeftPWM(0);
setRightPWM(0);
}
}
void trackLineInit()
{
// Assume we start on the center of the line
lineSlope = 0;
lineOffset = 0;
topLost = TRUE;
bottomLost = TRUE;
statsBufProcessed = TRUE;
// Set pan, tilt, boom postions
setServo(BOOM, BOOM_UP);
setServo(PAN, PAN_CENTER);
setServo(TILT, TILT_FORWARD);
myDelay(5);
// Initialize wheel speeds to some base speed
setLeftPWM(LEFT_BASE_SPEED);
setRightPWM(RIGHT_BASE_SPEED);
// Set Low Resolution
rprintf("hr 0\r");
// Downsample the image
rprintf("DS %d %d\r", DS_X_LINE, DS_Y_LINE);
// Line mode type 0, mode 2
// "per row statistics in the tracked region" p.41
rprintf("lm 0 2\r");
// Output mask hides everything but, mean p.45
rprintf("om 5 15\r");
// set black line tracking parameters p.51
rprintf("st 16 70 16 70 16 60\r");
// set virtual window
rprintf("vw %d %d %d %d\r", VW_X1_LINE, VW_Y1_LINE, VW_X2_LINE, VW_Y2_LINE);
// start the tracking (Track Color)
rprintf("tc\r");
}
void trackLine()
{
int8_t correction = 0;
forward();
/*
if (lineOffset >= -3 && lineOffset <= 3)
{
setRightPWM(BASE_SPEED);
setLeftPWM(BASE_SPEED);
}
else if (lineOffset < -3)
{
setRightPWM(50);
setLeftPWM(BASE_SPEED-20);
}
else
{
setRightPWM(BASE_SPEED-20);
setLeftPWM(50);
}
*/
if (!statsBufProcessed)
{
analyzeStatsBuf();
correction = MIN(MAX(lineSlope + lineOffset, -1*SPEED_RANGE), SPEED_RANGE);
setLeftPWM(LEFT_BASE_SPEED - correction);
setRightPWM(RIGHT_BASE_SPEED + correction);
lcdPrintHex( leftPWM, 1, 8);
lcdPrintHex(rightPWM, 1,11);
lcdPrintHex(correction, 1, 14);
/*
lcdPrintHex(statsBuf[0][X_MEAN], 1, 0);
lcdPrintHex(statsBuf[1][X_MIN], 1, 3);
lcdPrintHex(statsBuf[2][X_MAX], 1, 6);
lcdPrintHex(statsBuf[3][LINE_COUNT], 1, 9);
*/
/*
lcdWriteStr("l:", 1, 0);
lcdPrintHex(bottomPosition, 1, 2);
lcdWriteStr("o:", 1, 5);
lcdPrintHex(bottomOffset, 1, 7);
*/
}
/*
lcdWriteStr("left:", 0, 0);
lcdPrintHex(leftPWM, 0, 5);
lcdWriteStr("right:", 0, 8);
lcdPrintHex(rightPWM, 0, 14);
*/
}
//Set speed and direction of each engine
void Rover::interpretJoypadAxis(int id, qint16 value){
if(id==leftAxis){
qint16 valuePWM = ((double)value/std::numeric_limits<quint16>::max()) * 1000;
setLeftPWM(valuePWM);
}
if(id==rightAxis){
qint16 valuePWM = ((double)value/std::numeric_limits<quint16>::max()) * 1000;
setRightPWM(valuePWM);
}
}
