// Draw a little stick figure
void drawStickFigure(int offset)
{
nxtEraseRect(0, 0, 99, 50);
nxtDrawCircle(offset + 43, 40, 15);
nxtDrawLine(offset + 50,25,offset + 50,10);
nxtDrawLine(offset + 43, 22, offset + 58, 22);
nxtDrawLine(offset + 43, 0, offset + 50, 10);
nxtDrawLine(offset + 50, 10, offset + 58, 0);
}
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);
}
}
task main()
{
while(true){
chuck1val[0] = 0;
chuck1val[1] = 0;
chuck1val[2] = 0;
chuck1val[3] = 0;
chuck2val[0] = 0;
chuck2val[1] = 0;
buttonval[0] = 0;
buttonval[1] = 0;
NunchukGetParamter(S1, 0x00); //see comment for i2c_cmd
if (sizeof(outbuf)==6) {
// analog stick
eraseDisplay();
nxtDrawCircle((outbuf1[0]-0x1E)/2, (outbuf1[1]-0x1E)/3, 3);
nxtDisplayTextLine(2,"1 = %d", (outbuf1[3]-0x80));
chuck1val[2] = ((outbuf1[1])-0x80);
buttonval[0] = (outbuf1 [5] & 0x01);
buttonval[1] = (outbuf1 [5] & 0x02);
if(((outbuf1 [5]) & 0x01)==0){
chuck1val[0] = ((outbuf1[2])-0x80);
chuck1val[1] = ((outbuf1[3])-0x80);
}
else{
chuck1val[0] = 0;
chuck1val[1] = 0;
}
}
NunchukGetParamter(S2, 0x00); //see comment for i2c_cmd
if (sizeof(outbuf)==6) {
chuck1val[3] = ((outbuf1[1])-0x80);
buttonval[2] = (outbuf1 [5] & 0x01);
buttonval[3] = (outbuf1 [5] & 0x02);
chuck2val[0] = ((outbuf1[2])-0x80);
chuck2val[1] = ((outbuf1[3])-0x80);
}
checkBTLinkConnected();
eraseDisplay();
bNxtLCDStatusDisplay = true; // Enable top status line display
chuck1dat[0] = chuck1val[0]+100;
chuck1dat[1] = chuck1val[1]+100;
chuck1dat[2] = chuck1val[2]+100;
chuck1dat[3] = chuck1val[3]+100;
chuck2dat[0] = chuck2val[0]+100;
chuck2dat[1] = chuck2val[1]+100;
buttondat[0] = buttonval[0]+100;
buttondat[1] = buttonval[1]+100;
StartTask(sendMessages);
wait1Msec(1);
//return;
}
}
