task Lifter() { int Xdesired; // the encoder value of height we want to obtain int Xcurrent; //int lifterEncoderVal; int touchValTop; int touchValBottom; bool automode; int elevatorDirection = 1; int home = -5; automode = true; int joystickCurrent; while (true) { touchValBottom = SensorValue[TouchBottom]; touchValTop = SensorValue[TouchTop]; //following lines of code look to see if a button has been pressed. If it has, then it goes to the dictated position Xcurrent = nMotorEncoder[frontElevator]; Xdesired = readJoystick(Xcurrent); if (Xdesired != Xcurrent) elevatorGoToHeight(Xdesired); dispenseRings(); elevatorGoToHeight(home); } }
/*discretiza a posicao do joystick*/ int posicaoJoystick(char eixo){ uint16_t valor; if(eixo == 'x'){ valor = readJoystick('x'); }else if(eixo == 'y'){ valor = readJoystick('y'); } if(valor>3072){ return -1; //sentido positivo }else if(valor<1024){ return 1; //sentido negativo }else{ return 0; //nao pressionado } }
int elevatorGoToHeight(int Xdesired) { int Xcurrent; //int lifterEncoderVal; int touchValTop = 0; int touchValBottom = 0; bool automode; int elevatorDirection = 1; int proportionConstant = 1; // figure this out!!!! automode = true; Xcurrent = nMotorEncoder[frontElevator]; while(Xcurrent != Xdesired && touchValBottom == 0 && touchValTop == 0) { Xdesired = readJoystick(Xdesired); //reads controller to see if fickle user wants to go somewhere else instead //reads the touch value /* touchValBottom = SensorValue[TouchBottom]; touchValTop = SensorValue[TouchTop]; */ //reads where you are Xcurrent = nMotorEncoder[frontElevator]; //tells the motor to reverse if it overshoots if (Xcurrent > Xdesired) elevatorDirection = elevatorDirection * -1; //calculates what the distance you still need to go and determines the adjusted speed errorX = Xdesired - Xcurrent; Xspeed = proportionConstant * errorX * elevatorDirection; //corrects speed for distances over 100 if(abs(Xspeed) > 100) Xspeed = 100*elevatorDirection; //sets the speed of the motors motor[backElevator] = Xspeed*-1; motor[frontElevator] = Xspeed; } motor[backElevator] = 0; motor[frontElevator] = 0; if(joy2Btn(5)) { dispenseRings(); } return; }
task main() { initializeRobot(); // Execute robot initialization routine int Xcurrent, XcurrentTemp, Xdesired; waitForStart(); while(true) { getJoystickSettings(joystick); //clears the screen of automatic NXT diagnostic garbage bDisplayDiagnostics = false; bNxtLCDStatusDisplay = false; //DC motor controls //Left drive motor if(abs(joystick.joy1_y1) <= 15) //Joystick deadband { motor[Left1] = 0; motor[Left2] = 0; } else { motor[Left1] = joystick.joy1_y1*25/32; //Assign DriveMtrD (Motor 1) to the Left Y-Axis; scale joystick to motor (100/128) motor[Left2] = joystick.joy1_y1*25/32; //Assign DriveMtrD (Motor 1) to the Left Y-Axis; scale joystick to motor (100/128) } //Right drive motor if(abs(joystick.joy1_y2) <= 15) //Joystick deadband { motor[Right1] = 0; motor[Right2] = 0; } else { motor[Right1] = joystick.joy1_y2*25/32; //Assign DriveMtrE (Motor 2) to the Right Y-Axis; scale joystick to motor (100/128) motor[Right2] = joystick.joy1_y2*25/32; //Assign DriveMtrE (Motor 2) to the Right Y-Axis; scale joystick to motor (100/128) } //reads the protoboard to see which switches are pressed. Since they are globals, nothing is returned. We may want to make this it's own task eventually ProcessProto(); //only allows the elevator to go up if the bottom limit switch is pressed if(bottomLimit == 1) { if(joy2Btn(5)) { motor[elevatorA] = 85; motor[elevatorB] = 85; } else { motor[elevatorA] = 0; motor[elevatorB] = 0; } // uses opportunity to reset the encoder if it is all the way at the bottom to help agianst compiling error if (bottomLimit == 1) nMotorEncoder[elevatorA] = 0; } else //normal control if the bottom switch is not pressed { if(joy2Btn(5)) // Arm Up { motor[elevatorA] = 85; motor[elevatorB] = 85; } else if(joy2Btn(7)) // Arm Down { motor[elevatorA] = -55; motor[elevatorB] = -55; } else { motor[elevatorA] = 0; // Stop Arm Motion motor[elevatorB] = 0; } // uses opportunity to reset the encoder if it is all the way at the bottom to help agianst compiling error if (bottomLimit == 1) nMotorEncoder[elevatorA] = 0; } // Place RAM code here if(joy1Btn(2)) { nMotorEncoderTarget[RAMright] = 90; nMotorEncoderTarget[RAMright] = 90; ClearTimer(T1); while(nMotorEncoder[RAMright] < 90 && time1[T1] < 500) { motor[RAMright] = 50; motor[RAMleft] = 50; } motor[RAMright] = 0; motor[RAMleft] = 0; } if(joy1Btn(4)) { nMotorEncoderTarget[RAMright] = 0; nMotorEncoderTarget[RAMright] = 0; ClearTimer(T1); while(nMotorEncoder[RAMright] > 0 && time1[T1] < 1000) { motor[RAMright] = -65; motor[RAMleft] = -65; } motor[RAMright] = 0; motor[RAMleft] = 0; } // Place WAM code here // Place code to auto score a ring here // Place code to Pick-up a ring here Xcurrent = nMotorEncoder[elevatorA]; XcurrentTemp = Xcurrent; Xdesired = readJoystick(XcurrentTemp); //returns how many encoder counts need to go to if button pressed. If no button pressed, Xdesired stays the same if (Xdesired != Xcurrent) //if the user has changed the Xdesired fromt he current encoder position by pressing the preset button, then it does the p loop function to get to theat height { //PickUpRing(); // this funciton will use the p loop function to go until either the gripper hits the ground to pick up a singe ring or the limit switch in the arm is pressed to pick up two elevatorGoToHeight(Xdesired); } } }
/////////////////////////////////////////////////////////////////////////////////////////////// //This is the P loop to have the arm go to the desired location int elevatorGoToHeight(int Xdesired) //Xdesired is in encoder ticks { int XcurrentLocal; int errorX; int Xspeed; float proportionConstant = .2; // Proportional constant value. // Get current encoder value XcurrentLocal = nMotorEncoder[elevatorA]; while(Xdesired != XcurrentLocal) //only run the loop when you are not at the correct position. { XcurrentLocal = nMotorEncoder[elevatorA]; Xdesired = readJoystick(Xdesired); //reads controller to see if fickle user wants to change to another preset level. if no button is being pressed, Xdesired stays the same nxtDisplayTextLine(3, "AutoMode"); nxtDisplayTextLine(4, "Xcur %d", XcurrentLocal); nxtDisplayTextLine(5, "Xdes %d", Xdesired); //kicks out of the loop when you are close enough to the disired spot if ((XcurrentLocal < (Xdesired+150)) && (XcurrentLocal > (Xdesired-150))) { motor[elevatorA] = 0; motor[elevatorB] = 0; break; } //calculates how far from desired to determine how fast and what direction to go. errorX = Xdesired - XcurrentLocal; ////////////////////////////////////////////////////////// ////////// SETS THE SPEED ////////// ////////////////////////////////////////////////////////// Xspeed = (int) (proportionConstant * errorX); ////////////////////////////////////////////////////////// if (Xspeed > 0) //set min and max speed limits when going up { if (Xspeed > 90) Xspeed = 90; if (Xspeed < 40) Xspeed = 40; } if (Xspeed < 0) //set the min and max speed limits when going down { if (Xspeed < -55) Xspeed = -55; if (Xspeed > -15) Xspeed = -15; } //sets the speed of the motors to go to the position motor[elevatorA] = Xspeed; motor[elevatorB] = Xspeed; nxtDisplayTextLine(7, "speed %d", Xspeed); //stop button to cancel going to present loctation if(joy2Btn(8) == 1) { motor[elevatorA] = 0; motor[elevatorB] = 0; break; } } //turn off motors after leaving the while loop motor[elevatorA] = 0; motor[elevatorB] = 0; return 0; }
void main(){ //init pattern { asm("lda #$00"); //*(char*)(0x2006)=0; asm("sta $2006"); //*(char*)(0x2006)=0; asm("sta $2006"); //for(i=0;i<255;i++) asm("ldx #$00"); for1: //*(char*)(0x2007)=img1[i]; asm("lda %v,X",img1); asm("sta $2007"); asm("inx"); asm("cpx #$FF"); asm("bne %g",for1); //*(char*)(0x2007)=img1[255]; asm("lda %v,X",img1); asm("sta $2007"); //for(i=0;i<255;i++) asm("ldx #$00"); for2: //*(char*)(0x2007)=img2[i]; asm("lda %v,X",img2); asm("sta $2007"); asm("inx"); asm("cpx #$FF"); asm("bne %g",for2); //*(char*)(0x2007)=img1[255]; asm("lda %v,X",img2); asm("sta $2007"); } //clear SP { //*(char*)(0x2003)=0; //i<<2 asm("lda #$00"); asm("sta $2003"); //for(i=255;i>0;i--) asm("ldx #$FF"); for3: //*(char*)(0x2004)=0; asm("sta $2004"); asm("dex"); asm("bne %g",for3); //*(char*)(0x2004)=0; asm("sta $2004"); } //set background black { //*(char*)(0x2006)=0x20; asm("lda #$20");asm("sta $2006"); //*(char*)(0x2006)=0x20; asm("sta $2006"); //for(i=0;i<255;i++) asm("lda #$10"); asm("ldx #$FF"); for4: //*(char*)(0x2007)=0x10; asm("sta $2007"); asm("dex"); asm("bne %g",for4); //*(char*)(0x2007)=0x10; asm("sta $2007"); //for(i=0;i<255;i++) asm("ldx #$FF"); for5: //*(char*)(0x2007)=0x10; asm("sta $2007"); asm("dex"); asm("bne %g",for5); //*(char*)(0x2007)=0x10; asm("sta $2007"); //for(i=0;i<255;i++) asm("ldx #$FF"); for6: //*(char*)(0x2007)=0x10; asm("sta $2007"); asm("dex"); asm("bne %g",for6); //*(char*)(0x2007)=0x10; asm("sta $2007"); //for(i=0;i<128;i++) asm("ldx #$80"); for7: //*(char*)(0x2007)=0x10; asm("sta $2007"); asm("dex"); asm("bne %g",for7); } //set color { //*(char*)(0x2006)=0x3F; asm("lda #$3F");asm("sta $2006"); //*(char*)(0x2006)=0x00; asm("ldx #$00");asm("stx $2006"); //*(char*)(0x2007)=0x2C; asm("ldx #$2C");asm("stx $2007"); //*(char*)(0x2007)=0x30; asm("ldx #$30");asm("stx $2007"); //*(char*)(0x2007)=0x00; asm("ldx #$00");asm("stx $2007"); //*(char*)(0x2007)=0x3F; asm("sta $2007"); //*(char*)(0x2006)=0x3F; asm("sta $2006"); //*(char*)(0x2006)=0x11; asm("ldx #$11");asm("stx $2006"); //*(char*)(0x2007)=0x30; asm("ldx #$30");asm("stx $2007"); //*(char*)(0x2007)=0x22; asm("ldx #$22");asm("stx $2007"); //*(char*)(0x2007)=0x3F; asm("sta $2007"); } //show init title { //*(char*)(0x2006)=0x20; asm("lda #$20");asm("sta $2006"); //*(char*)(0x2006)=0xC5; asm("lda #$C5");asm("sta $2006"); //*(char*)(0x2007)=0x11; asm("ldx #$11");asm("stx $2007"); //*(char*)(0x2007)=0x12; asm("inx");asm("stx $2007"); //*(char*)(0x2007)=0x13; asm("inx");asm("stx $2007"); //*(char*)(0x2007)=0x14; asm("inx");asm("stx $2007"); //*(char*)(0x2007)=0x10; asm("lda #$10");asm("sta $2007"); //*(char*)(0x2007)=0x15; asm("inx");asm("stx $2007"); //*(char*)(0x2007)=0x10; asm("sta $2007"); //*(char*)(0x2007)=0x16; asm("inx");asm("stx $2007"); //*(char*)(0x2007)=0x17; asm("inx");asm("stx $2007"); //*(char*)(0x2007)=0x18; asm("inx");asm("stx $2007"); //*(char*)(0x2005) = 0; asm("stx $2005"); //*(char*)(0x2005) = 0; asm("stx $2005"); } //init ppu { //*(char*)(0x2000) = 0x80; //10 00 00 00 asm("lda #$80");asm("sta $2000"); //*(char*)(0x2001) = 0x18; //00 01 10 00 asm("lda #$18");asm("sta $2001"); //*(char*)(0x2005) = 0; asm("lda #$00");asm("sta $2005"); //*(char*)(0x2005) = 0; asm("sta $2005"); } //wait for key pressed (randomize) while1: rnd(); readJoystick(); asm("lda %b",key1); //if(key1)break; asm("bne %g",endwhile1); asm("jmp %g",while1); endwhile1: Clear(); //clear 4*4 nextShape { waitvblank(); asm("ldx #$00"); //*(char*)(0x2006)=0x20; asm("lda #$20");asm("sta $2006"); //*(char*)(0x2006)=0xAE; asm("lda #$AE");asm("sta $2006"); //*(char*)(0x2007)=0; asm("stx $2007"); //*(char*)(0x2007)=0; asm("stx $2007"); //*(char*)(0x2007)=0; asm("stx $2007"); //*(char*)(0x2007)=0; asm("stx $2007"); //*(char*)(0x2006)=0x20; asm("lda #$20");asm("sta $2006"); //*(char*)(0x2006)=0xCE; asm("lda #$CE");asm("sta $2006"); //*(char*)(0x2007)=0; asm("stx $2007"); //*(char*)(0x2007)=0; asm("stx $2007"); //*(char*)(0x2007)=0; asm("stx $2007"); //*(char*)(0x2007)=0; asm("stx $2007"); //*(char*)(0x2006)=0x20; asm("lda #$20");asm("sta $2006"); //*(char*)(0x2006)=0xEE; asm("lda #$EE");asm("sta $2006"); //*(char*)(0x2007)=0; asm("stx $2007"); //*(char*)(0x2007)=0; asm("stx $2007"); //*(char*)(0x2007)=0; asm("stx $2007"); //*(char*)(0x2007)=0; asm("stx $2007"); //*(char*)(0x2006)=0x21; asm("lda #$21");asm("sta $2006"); //*(char*)(0x2006)=0x0E; asm("lda #$0E");asm("sta $2006"); //*(char*)(0x2007)=0; asm("stx $2007"); //*(char*)(0x2007)=0; asm("stx $2007"); //*(char*)(0x2007)=0; asm("stx $2007"); //*(char*)(0x2007)=0; asm("stx $2007"); //*(char*)(0x2005) = 0; asm("stx $2005"); //*(char*)(0x2005) = 0; asm("stx $2005"); } NextShape(); NextShape(); //main cycle { //main_cnt = 50; asm("lda #$32"); asm("sta %b",main_cnt); while2: rnd(); waitvblank(); //one second count //if(main_cnt--==0) asm("dec %b",main_cnt); asm("bne %g",else1); //main_cnt=50; asm("lda #$32"); asm("sta %b",main_cnt); slowdown(); else1: readJoystick(); //key2 = key1 ^ lastkey; asm("lda %b",key1); asm("eor %b",lastkey); //key2 = key2 & key1; asm("and %b",key1); //if(key2!=0) asm("beq %g",else2); asm("sta %b",key2); //if(key2&0x01){movelr_n=1;movelr();}//button_RIGHT asm("and #$01"); asm("beq %g",elseKey1); asm("lda #$01"); asm("sta %b",movelr_n); movelr(); elseKey1: //if(key2&0x02){movelr_n=-1;movelr();}//button_LEFT asm("lda %b",key2); asm("and #$02"); asm("beq %g",elseKey2); asm("lda #$FF"); asm("sta %b",movelr_n); movelr(); elseKey2: //if(key2&0x04)down();//button_DOWN asm("lda %b",key2); asm("and #$04"); asm("beq %g",elseKey3); down(); elseKey3: //if(key2&0x08)slowdown();//button_UP asm("lda %b",key2); asm("and #$08"); asm("beq %g",elseKey4); slowdown(); elseKey4: //if(key2&0x10);//button_START //if(key2&0x20);//button_SELECT //if(key2&0x40){rotate_n=-1;rotate();}//button_B asm("lda %b",key2); asm("and #$40"); asm("beq %g",elseKey5); asm("lda #$FF"); asm("sta %b",rotate_n); rotate(); elseKey5: //if(key2&0x80){rotate_n=1;rotate();}//button_A asm("lda %b",key2); asm("and #$80"); asm("beq %g",elseKey6); asm("lda #$01"); asm("sta %b",rotate_n); rotate(); elseKey6: else2: //lastkey = key1; asm("lda %b",key1); asm("sta %b",lastkey); asm("jmp %g",while2); } }