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);
}
}
