task main(){
checkBTLinkConnected();
StartTask(sendMessages);
change(23,0,23,0,100,120,100,80);
wait1Msec(3000);
change(23,20,23,0,0,0,100,80);
wait1Msec(3000);
change(23,0,23,0,100,120,100,80);
wait1Msec(3000);
change(23,0,23,20,100,120,0,0);
wait1Msec(3000);
change(23,0,23,0,100,120,100,80);
wait1Msec(3000);
change(23,20,23,0,0,0,100,120);
wait1Msec(3000);
change(23,0,23,0,100,120,100,80);
wait1Msec(5000);
PlaySound(soundDownwardTones);
change(23,20,23,20,100,200,100,0);
wait1Msec(5000);
for(int i = 0; i<=3; ++i){
change(50,20,50,20,0,200,0,0);
wait1Msec(2000);
change(23,20,23,20,100,200,100,0);
wait1Msec(2000);
}
change(0,0,0,0,0,0,0,0);
wait1Msec(5000);
}
task main(){
checkBTLinkConnected();
dist = 0;
time = 0;
nMotorEncoder[motorA] = 0;
while(true){
wait1Msec(1);
dist = nMotorEncoder[motorA];
if(time == 100){
if(dist != 0){
sendMessageWithParm(dist);
}
time = 0;
}
nxtDisplayTextLine(1,"dist = %d", dist);
if(nNxtButtonPressed == kEnterButton) {
nMotorEncoder[motorA] = 0;
}
++time;
}
}
task main()
{
bNxtLCDStatusDisplay = true;
HTMCsetTarget(HTMC);
while(HTMCreadRelativeHeading(HTMC)>-90){
motor[motorD] = 15;
motor[motorE] = 15;
motor[motorF] = 15;
motor[motorG] = 15;
}
stopMotors();
HTMCsetTarget(HTMC);
wait10Msec(10);
while(true){
checkBTLinkConnected();
readMultipleDataMsgs();
wait1Msec(1);
}
return;
}
task main()
{
// while(true){
elbowdat[0] = -150; //elbow1 right-left
elbowdat[1] = -150; //elbow1 up-down
elbowdat[5] = 0; //elbow1 right-left
elbowdat[6] = 0; //elbow1 up-down
StartTask(sendMessages);
wait10Msec(100);
elbowdat[0] = -200; //elbow1 right-left
elbowdat[1] = -200; //elbow1 up-down
elbowdat[5] = 0; //elbow1 right-left
elbowdat[6] = 0; //elbow1 up-down
wait10Msec(100);
elbowdat[0] = -100; //elbow1 right-left
elbowdat[1] = -100; //elbow1 up-down
elbowdat[5] = 0; //elbow1 right-left
elbowdat[6] = 0; //elbow1 up-down
wait10Msec(100);
checkBTLinkConnected();
eraseDisplay();
bNxtLCDStatusDisplay = true; // Enable top status line display
wait1Msec(1);
//return;
}
task main(){
nMotorEncoder[UP] = 0;
nMotorEncoder[RND] = 0;
MUp = 0;
MRnd = 0;
distUp = 0;
distRnd = 0;
checkBTLinkConnected();
while(true){
wait1Msec(1);
//while (bQueuedMsgAvailable()){
//ClearMessage();
cCmdMessageRead(Rec, 2, mailbox1);
//Rec = messageParm[0];
//}
/*if(message == 0){
wait1Msec(5);
nxtDisplayTextLine(2,"no message");
continue;
}*/
nxtDisplayTextLine(1,"UP=%d,RND=%d",Rec[0], Rec[1]);
GUp = Rec[0];
GRnd = Rec[1];
MUp = GUp * 43;
MRnd = GRnd * 35;
nxtDisplayTextLine(2,"MUP=%d,MRND=%d",MUp, MRnd);
nxtDisplayTextLine(3,"UP = %d",nMotorEncoder[UP]);
nxtDisplayTextLine(4,"RND = %d",nMotorEncoder[RND]);
distUp = MUp-nMotorEncoder[UP];
if(MUp != nMotorEncoder[UP]){
motor[UP] = distUp;
}
else{
motor[UP] = 0;
}
distRnd = MRnd-nMotorEncoder[RND];
if(MRnd != nMotorEncoder[UP]){
motor[RND] = distRnd;
}
else{
motor[RND] = 0;
}
}
}
task main()
{
bNxtLCDStatusDisplay = true;
HTMCsetTarget(HTMC);
wait10Msec(10);
while(true){
checkBTLinkConnected();
readMultipleDataMsgs();
wait1Msec(1);
}
return;
}
task main() {
checkBTLinkConnected();
nMotorEncoder[UP] = 0;
nMotorEncoder[RND] = 0;
StartTask(get);
while(true) {
fRec[0] = (Rec[0]-8)*40;
fRec[1] = (Rec[1]-8)*40;
nxtDisplayTextLine(3,"RUP = %d",fRec[0]);
nxtDisplayTextLine(4,"RRND = %d",fRec[1]);
AbsMove(fRec[0], fRec[1]);
}
}
task main()
{
while(true){
//
// Test Ability to Turn Bluetooth On or Off
//
checkBTLinkConnected();
eraseDisplay();
bNxtLCDStatusDisplay = true; // Enable top status line display
readMessages();
wait1Msec(100);
return;
}
}
task main()
{
checkBTLinkConnected();
cCmdBTPurgeRcvBuffer();
purgeBT(mailbox1);
clearSendMessage();
while(true) {
clearBuffers();
readAndSend(mailbox1, mailbox11);
if(dataReceived[1]==1) {
send2=true;
PlaySound(soundFastUpwardTones);
}
clearBuffers();
}
}
task main(){
RgtMUp = 0;
RgtMRnd = 0;
LftMUp = 0;
LftMRnd = 0;
checkBTLinkConnected();
while(true){
wait1Msec(1);
while (bQueuedMsgAvailable()){
ClearMessage();
LftRec = messageParm[0];
RgtRec = messageParm[1];
}
/*if(message == 0){
wait1Msec(5);
nxtDisplayTextLine(2,"no message");
continue;
}*/
nxtDisplayTextLine(1,"message[0] = %d",LftRec);
nxtDisplayTextLine(2,"message[1] = %d",RgtRec);
LftGUp = LftRec/100;
LftGRnd = LftRec%100;
RgtGUp = RgtRec/100;
RgtGRnd = RgtRec%100;
RgtMUp = RgtGUp * 16;
RgtMRnd = (RgtGRnd * 16);
LftMUp = LftGUp * 16;
LftMRnd = (LftGRnd * 16);
nxtDisplayTextLine(3,"RGU = %d",RgtGUp);
nxtDisplayTextLine(4,"RGR = %d",RgtGRnd);
nxtDisplayTextLine(5,"LGU = %d",LftGUp);
nxtDisplayTextLine(6,"LGR = %d",LftGRnd);
servo[srvo_S1_C2_1] = RgtMUp;
servo[srvo_S1_C2_2] = RgtMRnd;
servo[srvo_S1_C2_3] = LftMUp;
servo[srvo_S1_C2_4] = LftMRnd;
}
}
task main(){
checkBTLinkConnected();
StartTask(sendMessages);
change(6,8,6,8,100,150,100,200);
wait1Msec(3000);
change(10,13,6,8,200,0,100,200);
wait1Msec(1000);
change(6,8,6,8,100,150,100,200);
wait1Msec(1000);
change(6,13,6,8,200,0,100,200);
wait1Msec(1000);
change(6,8,6,8,100,150,100,200);
wait1Msec(1000);
change(3,13,6,8,200,0,100,200);
wait1Msec(1000);
change(6,8,6,8,100,150,100,200);
wait1Msec(1500);
change(6,8,10,13,100,150,200,255);
wait1Msec(1000);
change(6,8,6,8,100,150,100,200);
wait1Msec(1000);
change(6,8,6,13,100,150,200,255);
wait1Msec(1000);
change(6,8,6,8,100,150,100,200);
wait1Msec(1000);
change(6,8,3,13,100,150,200,255);
wait1Msec(1000);
change(6,8,6,8,100,0,100,200);
wait1Msec(2000);
PlaySound(soundDownwardTones);
change(6,10,6,10,100,0,100,255);
wait1Msec(1500);
for(int i = 0; i<=10; ++i){
change(12,10,12,10,200,0,200,255);
wait1Msec(1000);
change(6,10,6,10,100,0,100,255);
wait1Msec(1000);
}
change(0,8,0,8,200,0,200,0);
wait1Msec(5000);
}
task main()
{
char inValue = 0;
char inLastValue = 0;
string PIN = 1234;
char nIndex = 0;
char oIndex = 0;
int touch01 = 0;
int touch02 = 0;
int sonar01 = 0;
const int kHexDigitsPerLine = 2;
ubyte MarkerRead[kHexDigitsPerLine];
ubyte SensorRead[kHexDigitsPerLine];
int usDistance = 0;
//Establish arduino connection
btConnect(3, "SeeedBTSlave");
wait1Msec(1000);
setSessionPIN(PIN);
nxtDisplayCenteredBigTextLine(2, "Testing!");
wait1Msec(5000);
//Check that the connection is live
checkBTLinkConnected();
//Push bluecore into RAW mode
nxtDisplayCenteredTextLine(4, "Before RAW");
setBluetoothRawDataMode();
while (!bBTRawMode)
{
nxtDisplayCenteredTextLine(4, "Inside RAW");
wait1Msec(1);
}
nxtDisplayCenteredTextLine(4, "After RAW");
//Send some data to the connected arduino
ubyte count[3];
ubyte count2[3];
count[0]=70;
count[1]= 65;
count[2]= 66;
count2[0]=65;
count2[1]=70;
count2[2]=71;
nxtWriteRawBluetooth(count, sizeof(count));
wait1Msec(1000);
nxtWriteRawBluetooth(count2, sizeof(count2));
wait1Msec(1000);
eraseDisplay();
while(true)
{
nxtDisplayCenteredTextLine(1, "Reading");
string str = "";
nxtReadRawBluetooth(&MarkerRead[1], 2);
inValue = MarkerRead[1];
if (inValue == 63)
{
nxtReadRawBluetooth(&SensorRead[1], 2);
nIndex = SensorRead[1];
if ( ( nIndex >= 65 ) && ( nIndex <= 70 ) )
{
if (nIndex == 65 ){touch01 = 1;nxtDisplayTextLine(2, "One is ON: %d", touch01);}
else if (nIndex == 66 ){touch01 = 0;nxtDisplayTextLine(2, "One is OFF: %d", touch01);}
else if (nIndex == 67 ){touch02 = 1;nxtDisplayTextLine(3, "Two is ON: %d", touch02);}
else if (nIndex == 68 ){touch02 = 0;nxtDisplayTextLine(3, "Two is OFF: %d", touch02);}
else if (nIndex == 69 ){sonar01 = 0;nxtDisplayTextLine(4, "Sonar1 OUT: %d", sonar01);}
else if (nIndex == 70 ){sonar01 = 1;nxtDisplayTextLine(4, "Sonar1 IN: %d", sonar01);}
nIndex = oIndex;
}
}
}
}
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;
}
}
task sendMessages()
{
const int kMinTimeBetweenMsg = 50;
static long nLastSendTime = 0;
while (true)
{
//
// Delay between messages.
//
checkBTLinkConnected();
if ((nPgmTime - nLastSendTime) < kMinTimeBetweenMsg)
{
wait1Msec(6);
continue;
}
if (bBTBusy)
{
++nSendBusy;
wait1Msec(5);
continue;
}
++nNearEndSent;
++nMsgXmit;
//if (nMsgXmit == 0)
nMsgXmit = 1; // Don't send illegal value
ubyte krik[4]={0,0,0,0};
if(cmps==0){
krik[0] = 0;
krik[1] = 0;
}
if(cmps>0){
krik[0] = cmps;
krik[1] = 0;
}
if(cmps<0){
krik[1] = -cmps;
krik[0] = 0;
}
NunchukGetParamter(S3, 0x00);
if (sizeof(outbuf)==6) {
if((outbuf1[5]&0x02)!=0){
krik[2] = 1;
}
if((outbuf1[5]&0x01)!=0){
krik[3] = 1;
}
}
cCmdMessageWriteToBluetooth(3,krik,4,mailbox1);
wait10Msec(1);
nLastSendTime = nPgmTime;
}
return;
}
task main (){
checkBTLinkConnected();
LftAccumulatorRound=0;
LftHeadingRound = 0;
RgtAccumulatorRound=0;
RgtHeadingRound = 0;
time = 0;
nTime = 0;
cTime = 0;
LftSendFin = 0;
RgtSendFin = 0;
LstLftSendRound = 0;
LstRgtSendRound = 0;
HTGYROstartCal(LFTGYROROUND);
HTGYROstartCal(RGTGYROROUND);
while(true){
wait1Msec(1);
LftAccumulatorRound+=HTGYROreadRot(LFTGYROROUND);
RgtAccumulatorRound+=HTGYROreadRot(RGTGYROROUND);
if(time == 10){
eraseDisplay();
LftHeadingRound += LftAccumulatorRound;
RgtHeadingRound += RgtAccumulatorRound;
if(LftHeadingRound > 80000)
LftHeadingRound = 80000;
if(LftHeadingRound < -80000)
LftHeadingRound = -80000;
if(RgtHeadingRound < -80000)
RgtHeadingRound = -80000;
if(RgtHeadingRound > 80000)
RgtHeadingRound = 80000;
LftSendRound = (LftHeadingRound*0.0001)+8;
RgtSendRound = (RgtHeadingRound*0.0001);
RgtSendRound = 0-RgtSendRound+8;
if(LftSendRound <= 16 && LftSendRound > 0)
LftSendFin[0] = LftSendRound;
if(RgtSendRound <= 16 && RgtSendRound > 0)
RgtSendFin[0] = RgtSendRound;
FinSendfin[0] = LftSendFin[0];
FinSendfin[1] = RgtSendFin[0];
if(nTime == 10){
cCmdMessageWriteToBluetooth(1,FinSendfin,2,mailbox1);
wait1Msec(1);
nTime = 0;
}
/*if(cTime == 200){
if(LftSendRound == LstLftSendRound){
PlaySound(soundException);
HTGYROstartCal(LFTGYROROUND);
}
else
LstLftSendRound = LftSendRound;
if(RgtSendRound == LstRgtSendRound){
PlaySound(soundException);
HTGYROstartCal(RGTGYROROUND);
}
else
LstRgtSendRound = RgtSendRound;
cTime = 0;
}*/
nxtDisplayTextLine(1,"RND = %4d, %4d",FinSendfin[0],FinSendfin[1]);
nxtDisplayTextLine(3,"Press [] to reset");
LftAccumulatorRound = 0;
RgtAccumulatorRound = 0;
time = 0;
++nTime;
++cTime;
}
if(nNxtButtonPressed == kEnterButton) {
LftHeadingRound = 0;
HTGYROstartCal(LFTGYROROUND);
RgtHeadingRound = 0;
HTGYROstartCal(RGTGYROROUND);
}
++time;
}
}