//lim1 is my limit switch and leftMotor is my motor im using
int armPos = 0; //This is position of the arm. it is found by Task armCount
int dir = 1; // this is the direction for use in Task armCount. (1 its moving clockwise -1 for counter clockwise

void move (int goTo) //this is supposed to be a function for moving my arm while also keeping up with the direction
{
  while (armPos != goTo) // this is supposed to be a while loop that as long as the armPos in not equal to where I want the arm to go
  {
    if (armPos < goTo)  //this is supposed to say if the armPos is less than the number position I want it to go to
    {
      dir = 1;  // this is supposed to make dir equal to 1
      motor[leftMotor] = 127; // this is supposed to make the motor go full powe forward
    }
    else if (armPos > goTo) //this is supposed to say if the armPos is greater than the number position I want it to go to
    {
      dir = -1; // this is supposed to make dir equal to -1
      motor[leftMotor] = -127; // this is supposed to make the motor go full power reverse
    }
    else if (armPos == goTo)
    {
      //Do Nothing
    }
  }
}
   


task armCount() //this is code by "magicode" and seems to work very well
{
 while(true){
    while(SensorValue(lim1) == 0)
    {
      wait1Msec(1);
    }
    while(SensorValue(lim1) == 1)
    {
      wait1Msec(1);
    }
    hogCPU();
    armPos = (armPos + dir); // i edited it right here from armPos++; so that if dir is negitive it subtract one from armPos keeping up with the position of the arm
    releaseCPU();
  }
}


task main()
{
  StartTask(armCount); // this starts the task armCount which it seems to do perfectly
  while(true)
  {
 
    if (vexRT[Btn7U]==1) //this is supposed to make the arm go to position 4 if  Button 7U is pushed
    {
      move(4);
    }
    if (vexRT[Btn7R] == 1) //same as above but to position 2
    {
      move(2);
    }
    if (vexRT[Btn7D] == 1 )// same as above but to positon 6
    {
      move(6);
    }
  }
}
   

//the name of the limit switch is "lim"
int armPos = 0;

//keeps count of the armPosition
task encoderCount(){
  while(true){
    while(SensorValue(lim) == 0){
      wait1Msec(1);
    }
    while(sensorValue(lim) == 1){
      wait1Msec(1);
    }
    hogCPU();
    armPos++;
    releaseCPU();
  }
}

task main(){
  StartTask(encoderCount);
  while(armPos < 40){
    motor[arm] = 127;
  }
}

//Notes:
//  * Switches are lim and lim2
//  * lim is the upper switch in the diagram, and lim2 is the lower switch

int direction = 0; //0 if not moving, 1 if moving clockwise, 2 if moving counter-clockwise

void waitRelease(int switch)
{
  if (switch == 1)
  {
    while(SensorValue[lim] == 1)
    {
      //Do nothing
    }
  }
  else if (switch == 2)
  {
    while(SensorValue[lim2] == 1)
    {
      //Do nothing
    }
  }
}

task direction()
{
  //These two variables will be equal to 1 or 2, depending on which limit was pressed first.
  int lastPress = 1;
  int pressed = 1;
 
  int i = 0;

  while(true)
  {
    lastPress = pressed;

    if(SensorValue[lim] == 1)
    {
      pressed = 1;
      waitRelease(1);
    }
    else if(SensorValue[lim2] == 1)
    {
      pressed = 2;
      waitRelease(2);
    }
   
    i++;

    if (i=1) exitLoop(); //Not sure what the real code is for this...

    if ((lastPress - pressed) == 1)
    {
      //Clockwise
      direction = 1;
    }
    else if((lastPress - pressed) == -1)
    {
      //Counter-clockwise
      direction = 2;
    }
    else if((lastPress - pressed) == 0)
    {
      //Not moving
      direction = 0;
    }
  }
}