ROBOTC.net forums
http://www.robotc.net/forums/

Would this code work?
http://www.robotc.net/forums/viewtopic.php?f=52&t=738
Page 1 of 1

Author:  CC335 [ Tue Sep 23, 2008 3:33 pm ]
Post subject:  Would this code work?

Note that I will be using a lego pnumatic system actuated by Servo3. Servos 1 and 2 are constant rotation servos.
Here's my code:
Code:
#pragma config(Hubs,  S1, HTMotor,  HTServo,  none,     none)
#pragma config(Sensor, S3,     Light,               sensorLightActive)
#pragma config(Sensor, S4,     Sonar,               sensorSONAR)
#pragma config(Motor,  motorB,          ,              tmotorNormal, PIDControl, reversed)
#pragma config(Motor,  mtr_S1_C1_1,     motor1,        tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C1_2,     motor2,        tmotorNormal, openLoop)
#pragma config(Servo,  servo1,          Servo1,               tServoNormal)
#pragma config(Servo,  servo2,          Servo2,               tServoNormal)
#pragma config(Servo,  servo3,          Servo3,               tServoNormal)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

int autotimer;
int mode2;

#include "JoystickDriver.c"

task main()

{
  autotimer = 0;
  mode2 = 1;
  motor[motorB] = 100;
  while (autotimer < 300)
  {

     if (mode2 == 1)
     {
       if (SensorValue(Sonar) < 30)
       {
          mode2 = 2;
      }
    }
     if (mode2 == 1)
     {
       motor[motor1] = 100;
       motor[motor2] = 100;
    }
    if (mode2 == 2)
    {
       motor[motor1] = 75;
       motor[motor2] = - 75;
       wait10Msec (85);
       mode2 = 3;
       autotimer = autotimer + 10;
    }
    if (mode2 == 3)
    {
       motor[motor1] = 100;
       motor[motor2] = 100;
       if (SensorValue(Light) < 25)
       {
         wait10Msec (300);
         motor[motor1] = - 75;
         motor[motor2] = 75;
         wait10Msec (85);
         autotimer = autotimer + 40;
         motor[motor1] = 100;
         motor[motor2] = 100;
         wait10Msec (100);
         autotimer = autotimer + 10;
         mode2 = 4;
      }
    }
    if (mode2 == 4)
    {
       motor[motor1] = 0;
       motor[motor2] = 0;
       servo[Servo3] = 750;
       wait10Msec (300);
       servo[Servo3] = 0;
       servo[Servo1] = 750;
       servo[Servo2] = - 750;
       wait10Msec (300);
       servo[Servo3] = - 750;
       wait10Msec (200);
       servo[Servo3] = 0;
       servo[Servo1] = 0;
       servo[Servo2] = 0;
       autotimer = 300;
    }
    wait10Msec (9);
    ++autotimer;
  }


  while(true)
  {
    while (true)
    {
      getJoystickSettings(joystick);
      motor[motor1] = joystick.joy1_y1;
      motor[motor2] = joystick.joy2_y1;

      if(joy1Btn(1))
      {
         servo[Servo3] = 750;
      }
      else
      {
         servo[Servo3] = 0;
      }
      if (joy1Btn(3))
      {
         servo[Servo3] = - 750;
      }
      if(joy2Btn(1))
      {
         servo[Servo1] = 750;
         servo[Servo2] = - 750;
      }
      else
      {
         servo[Servo1] = 0;
         servo[Servo2] = 0;
      }
      if(joy2Btn(3))
      {
         servo[Servo1] = - 750;
         servo[Servo2] = 750;
      }
   }
}

Author:  tfriez [ Wed Sep 24, 2008 12:49 pm ]
Post subject:  Re: Would this code work?

Few notes:

1. There are timers built into ROBOTC. You can access them with the time1[T1], time10[T1], time100[T1]... the number after the word time is the millisecond counter increment (1 = 1ms, 10 = 10ms, 100= 10ms). So for example, you wanted a loop to run for 30 seconds, you could use the following

Code:
ClearTimer(T1);

while(time1[T1] < 30000)
{

}


2. You have a double while loop at the bottom half of your code... this code will repeat forever. Not sure if this is the desired result.

3. This isn't how the "FTC" system should work. You have to wait for the field management system to "enable" your robot in autonomous. Look at the www.ftctraining.com website for sample code on how to do this.

Author:  CC335 [ Wed Sep 24, 2008 3:10 pm ]
Post subject:  Re: Would this code work?

It won't let me download the code...

Still, I used your code to corect mine:
Code:
#pragma config(Hubs,  S1, HTMotor,  HTServo,  HTMotor,  none)
#pragma config(Sensor, S3,     Light,               sensorLightActive)
#pragma config(Sensor, S4,     Sonar,               sensorSONAR)
#pragma config(Motor,  mtr_S1_C1_1,     motor1,        tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C1_2,     motor2,        tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C3_1,     motor3,        tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C3_2,     motor4,        tmotorNormal, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

int mode2;

#include "JoystickDriver.c"

task main()

{
  mode2 = 1;
  servoTarget[servo1] = 0;
  servoTarget[servo2] = 0;
  servoTarget[servo3] = 0;

  ClearTimer(T1);

  getJoystickSettings(joystick);  //Updates the game and joystick variables

        while(joystick.StopPgm)  //Continues this loop while the robot is disabled (StopPgm = true)
        {
           getJoystickSettings(joystick);  //Checks to see if the robot has been enabled (StopPgm = false)
           wait1Msec(50);  //Waits 50 milliseconds for a new BT packet to come.
        }

       // Autonomous Code Below - You can put all of your autonomous program below

  while (time1[T1] < 30000)
  {

     if (mode2 == 1)
     {
       if (SensorValue(Sonar) < 30)
       {
          mode2 = 2;
      }
    }
     if (mode2 == 1)
     {
       motor[motor1] = 100;
       motor[motor2] = 100;
    }
    if (mode2 == 2)
    {
       motor[motor1] = 75;
       motor[motor2] = - 75;
       wait10Msec (85);
       mode2 = 3;
    }
    if (mode2 == 3)
    {
       motor[motor1] = 100;
       motor[motor2] = 100;
       if (SensorValue(Light) < 25)
       {
         wait10Msec (300);
         motor[motor1] = 75;
         motor[motor2] = - 75;
         wait10Msec (85);
         motor[motor1] = 100;
         motor[motor2] = 100;
         wait10Msec (100);
         mode2 = 4;
      }
    }
    if (mode2 == 4)
    {
       motor[motor1] = 0;
       motor[motor2] = 0;
       servoTarget[servo3] = 750;
    }
    wait10Msec (9);
  }

   motor[motor1] = 50;
   motor[motor2] = 50;
   wait1Msec(3000);

   motor[motor1] = 0;
   motor[motor2] = 0;

  while(true)
  {
    getJoystickSettings(joystick);
    motor[motor1] = joystick.joy1_y1;
    motor[motor2] = joystick.joy2_y1;

    if(joy1Btn(1))
    {
      servoTarget[servo3] = 750;
    }
    else
    {
      servoTarget[servo3] = 0;
    }
    if(joy2Btn(1))
    {
      motor[motor3] = 100;
      motor[motor4] = - 100;
    }
    else
    {
      motor[motor3] = 0;
      motor[motor4] = 0;
    }
    if(joy1Btn(3))
    {
      servoTarget[servo1] = 750;
      servoTarget[servo2] = - 750;
    }
     else
    {
      servoTarget[servo1] = 0;
      servoTarget[servo2] = 0;
    }
  }
}

I also corected the design of the robot. Apparently you can't use Lego pnumatic sets or tank tracks.

Author:  tfriez [ Wed Sep 24, 2008 4:19 pm ]
Post subject:  Re: Would this code work?

Compiles for me... I added a few notes to your program.

Code:
#pragma config(Hubs,  S1, HTMotor,  HTServo,  HTMotor,  none)
#pragma config(Sensor, S3,     Light,               sensorLightActive)
#pragma config(Sensor, S4,     Sonar,               sensorSONAR)
#pragma config(Motor,  mtr_S1_C1_1,     motor1,        tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C1_2,     motor2,        tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C3_1,     motor3,        tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C3_2,     motor4,        tmotorNormal, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

int mode2;

#include "JoystickDriver.c"

task main()
{
  mode2 = 1;
  servoTarget[servo1] = 0;
  servoTarget[servo2] = 0;
  servoTarget[servo3] = 0;

  ClearTimer(T1);

  getJoystickSettings(joystick);  //Updates the game and joystick variables

        while(joystick.StopPgm)  //Continues this loop while the robot is disabled (StopPgm = true)
        {
           getJoystickSettings(joystick);  //Checks to see if the robot has been enabled (StopPgm = false)
           wait1Msec(50);  //Waits 50 milliseconds for a new BT packet to come.
        }

       // Autonomous Code Below - You can put all of your autonomous program below

  while (true) //Don't need timing - separate autonomous and user control programming
  {
     if (mode2 == 1)
     {
       if (SensorValue(Sonar) < 30)
       {
          mode2 = 2;
      }
    }
     if (mode2 == 1)
     {
       motor[motor1] = 100;
       motor[motor2] = 100;
    }
    if (mode2 == 2)
    {
       motor[motor1] = 75;
       motor[motor2] = - 75;
       wait10Msec (85);
       mode2 = 3;
    }
    if (mode2 == 3)
    {
       motor[motor1] = 100;
       motor[motor2] = 100;
       if (SensorValue(Light) < 25)
       {
         wait10Msec (300);
         motor[motor1] = 75;
         motor[motor2] = - 75;
         wait10Msec (85);
         motor[motor1] = 100;
         motor[motor2] = 100;
         wait10Msec (100);
         mode2 = 4;
      }
    }
    if (mode2 == 4)
    {
       motor[motor1] = 0;
       motor[motor2] = 0;
       servoTarget[servo3] = 750;
    }
    // wait10Msec (9); - why wait 90 milliseconds on every loop?
  }

   motor[motor1] = 50;
   motor[motor2] = 50;
   wait1Msec(3000);

   motor[motor1] = 0;
   motor[motor2] = 0;



/////////// Make this a seperate program //////////
  while(true)
  {
    getJoystickSettings(joystick);
    motor[motor1] = joystick.joy1_y1;
    motor[motor2] = joystick.joy2_y1;

    if(joy1Btn(1))
    {
      servoTarget[servo3] = 750;
    }
    else
    {
      servoTarget[servo3] = 0;
    }
    if(joy2Btn(1))
    {
      motor[motor3] = 100;
      motor[motor4] = - 100;
    }
    else
    {
      motor[motor3] = 0;
      motor[motor4] = 0;
    }
    if(joy1Btn(3))
    {
      servoTarget[servo1] = 750;
      servoTarget[servo2] = - 750;
    }
     else
    {
      servoTarget[servo1] = 0;
      servoTarget[servo2] = 0;
    }
  }
}

Author:  CC335 [ Wed Sep 24, 2008 5:02 pm ]
Post subject:  Re: Would this code work?

I couldn't get it to work with two seprate programs, but here is the optimised code:
Code:
#pragma config(Hubs,  S1, HTMotor,  HTServo,  HTMotor,  none)
#pragma config(Sensor, S3,     Light,               sensorLightActive)
#pragma config(Sensor, S4,     Sonar,               sensorSONAR)
#pragma config(Motor,  mtr_S1_C1_1,     motor1,        tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C1_2,     motor2,        tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C3_1,     motor3,        tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C3_2,     motor4,        tmotorNormal, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

int mode2;

#include "JoystickDriver.c"

task main()

{
  mode2 = 1;
  servoTarget[servo1] = 0;
  servoTarget[servo2] = 0;
  servoTarget[servo3] = 0;

  ClearTimer(T1);

  getJoystickSettings(joystick);  //Updates the game and joystick variables

        while(joystick.StopPgm)  //Continues this loop while the robot is disabled (StopPgm = true)
        {
           getJoystickSettings(joystick);  //Checks to see if the robot has been enabled (StopPgm = false)
           wait1Msec(50);  //Waits 50 milliseconds for a new BT packet to come.
        }

       // Autonomous Code Below - You can put all of your autonomous program below

  while (time1[T1] < 30000)
  {

     if (mode2 == 1)
     {
       if (SensorValue(Sonar) < 30)
       {
          mode2 = 2;
      }
    }
     if (mode2 == 1)
     {
       motor[motor1] = 100;
       motor[motor2] = 100;
    }
    if (mode2 == 2)
    {
       motor[motor1] = 75;
       motor[motor2] = - 75;
       wait10Msec (85);
       mode2 = 3;
    }
    if (mode2 == 3)
    {
       motor[motor1] = 100;
       motor[motor2] = 100;
       if (SensorValue(Light) < 30)
       {
         wait10Msec (300);
         motor[motor1] = 75;
         motor[motor2] = - 75;
         wait10Msec (85);
         motor[motor1] = 100;
         motor[motor2] = 100;
         wait10Msec (100);
         mode2 = 4;
      }
    }
    if (mode2 == 4)
    {
       motor[motor1] = 0;
       motor[motor2] = 0;
       servoTarget[servo3] = 750;
    }
    wait1Msec (9);
  }

   motor[motor1] = 50;
   motor[motor2] = 50;
   wait1Msec(3000);

   motor[motor1] = 0;
   motor[motor2] = 0;

   while(true)
    {
      getJoystickSettings(joystick);
      motor[motor1] = joystick.joy1_y1;
      motor[motor2] = joystick.joy2_y1;

      if(joy1Btn(1))
      {
        servoTarget[servo3] = 750;
      }
      else
      {
        servoTarget[servo3] = 0;
      }
      if(joy2Btn(1))
      {
        motor[motor3] = 100;
        motor[motor4] = - 100;
      }
      else
      {
        motor[motor3] = 0;
        motor[motor4] = 0;
      }
      if(joy1Btn(3))
      {
        servoTarget[servo1] = 750;
        servoTarget[servo2] = - 750;
      }
      else
      {
        servoTarget[servo1] = 0;
        servoTarget[servo2] = 0;
      }

  }
}

Btw, I prefer to have the timer code on the robot. It's just there so that I can make sure that the robot goes into tele-opp mode as soon as auto mode ends.

Author:  Dick Swan [ Sat Sep 27, 2008 3:22 am ]
Post subject:  Re: Would this code work?

You probably want to post a question on the "US First" site regarding whether it's a legal modifcation to use "continuous rotation" servos. I suspect the reply is no. At least that is what I recall when I informally asked this question earlier this summer.

Author:  CC335 [ Sat Sep 27, 2008 3:37 pm ]
Post subject:  Re: Would this code work?

Don't worry. I saw that you have to use the servos included in the kit, so I changed the code. I am writing a new (larger) code (that uses multiple include files that I am writing) anyway.

Author:  CC335 [ Sun Sep 28, 2008 7:53 pm ]
Post subject:  Re: Would this code work?

Let me just post an update on my program. It is a new program that I wrote.
This is the main file:
Code:
#pragma config(WizType, FTC, 2, Tankbot, Analog)
#pragma config(Hubs,  S1, HTMotor,  HTServo,  none,     none)
#pragma config(Hubs,  S2, HTMotor,  none,     none,     none)
#pragma config(Driving, Tankbot, Y1, X1)
#pragma config(Sensor, S3,     Light,               sensorLightActive)
#pragma config(Sensor, S4,     Sonar,               sensorSONAR)
#pragma config(Motor,  mtr_S1_C1_1,     motor1,        tmotorNormal, PIDControl)
#pragma config(Motor,  mtr_S1_C1_2,     motor2,        tmotorNormal, PIDControl)
#pragma config(Motor,  mtr_S2_C1_1,     motor3,        tmotorNormal, PIDControl)
#pragma config(Motor,  mtr_S2_C1_2,     motor4,        tmotorNormal, PIDControl)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

// #pragma config is for motor setings and is NOT to be edited unless the design of the robot completely changes.

#include "HeaderFTC.c" // include the FTC header file
#include "teleopHeader.c" // include teleop code
task main() // main program
{
   startup();
  getJoystickSettings(joystick); // get joystick setings
  if(joystick.UserMode == false) // the following code is hard to explain
  {
     if(mode1 == false) // red code...
     {
       if (mode2 == 1)
        {
        if (SensorValue(Sonar) < 30)
        {
           mode2 = 2;
       }
       }
        if (mode2 == 1)
        {
          motor[motor1] = 100;
          motor[motor2] = 100;
       }
       if (mode2 == 2)
       {
          motor[motor1] = - 75;
          motor[motor2] = 75;
          wait10Msec (85);
          mode2 = 3;
       }
       if (mode2 == 3)
       {
          motor[motor1] = 100;
          motor[motor2] = 100;
          if (SensorValue(Light) < 30)
        {
             wait10Msec (300);
             motor[motor1] = - 75;
             motor[motor2] = 75;
             wait10Msec (85);
             motor[motor1] = 100;
             motor[motor2] = 100;
             wait10Msec (100);
             mode2 = 4;
          }
        }
        if (mode2 == 4)
        {
           motor[motor1] = 0;
           motor[motor2] = 0;
           servoTarget[servo3] = 200;
        }
        wait1Msec (9);
      }
      else // blue code...
      {
         if (mode2 == 1)
         {
           if (SensorValue(Sonar) < 30)
           {
              mode2 = 2;
          }
        }
         if (mode2 == 1)
         {
           motor[motor1] = 100;
           motor[motor2] = 100;
        }
        if (mode2 == 2)
        {
           motor[motor1] = 75;
           motor[motor2] = - 75;
           wait10Msec (85);
           mode2 = 3;
        }
        if (mode2 == 3)
        {
           motor[motor1] = 100;
           motor[motor2] = 100;
           if (SensorValue(Light) < 30)
           {
             wait10Msec (300);
             motor[motor1] = 75;
             motor[motor2] = - 75;
             wait10Msec (85);
             motor[motor1] = 100;
             motor[motor2] = 100;
             wait10Msec (100);
             mode2 = 4;
          }
        }
        if (mode2 == 4)
        {
           motor[motor1] = 0;
           motor[motor2] = 0;
           servoTarget[servo3] = 200;
        }
        wait1Msec (9);
      }
    }
    else // and Tele-op code
    {
      teleop();
    }
} // end of file

This is the 'HeaderFTC.c' file:
Code:
#include "JoystickDriver.c" // include the Joystick Driver file

bool mode1;
int mode2;

void startup()
{
   mode2 = 1;
   btConnect(1, "AncientMinister"); // connect to computer
   getJoystickSettings(joystick); // Get the joystick setings and store it in variable "joystick"
  while(joystick.StopPgm == true) //the following code is to keep checking for a signal from the FTC control panel
  {
     getJoystickSettings(joystick);
     wait10Msec(5);
  }
  if (joystick.TeamColor == false) // If team color is red...
  {
     mode1 = false; // set robot mode to red
  }
  else // otherwise...
  {
     mode1 = true; // set to blue
  }
  return; // go to main file.
}

And this is the 'teleopHeader.c' file
Code:
void teleop()
{
  while(true)
       {
         getJoystickSettings(joystick);
        motor[motor1] = joystick.joy1_y1;
        motor[motor2] = joystick.joy2_y1;

        if(joy1Btn(1))
        {
          servoTarget[servo3] = 200;
        }
        else
        {
          servoTarget[servo3] = 0;
        }
        if(joy2Btn(1))
        {
          motor[motor3] = 100;
          motor[motor4] = - 100;
        }
        else
        {
          motor[motor3] = 0;
          motor[motor4] = 0;
        }
        if(joy1Btn(3))
        {
          servoTarget[servo1] = 200;
          servoTarget[servo2] = - 200;
        }
        else
        {
          servoTarget[servo1] = 0;
          servoTarget[servo2] = 0;
        }
      }
  }

Author:  Dick Swan [ Mon Sep 29, 2008 2:56 am ]
Post subject:  Re: Would this code work?

Quote:
Code:
  if (joystick.TeamColor == false) // If team color is red...
  {
     mode1 = false; // set robot mode to red
  }
  else // otherwise...
  {
     mode1 = true; // set to blue
  }

Unofrtunately, it was decided that the team color field will not be included during a competition in the PC program that manages a game. It would be best to ignore this field.

Author:  CC335 [ Mon Sep 29, 2008 10:26 pm ]
Post subject:  Re: Would this code work?

Thanks. That will help a LOT! :programmer:

Author:  XBHS [ Wed Oct 22, 2008 3:35 pm ]
Post subject:  Re: Would this code work?

Looks like it would you might want to streamline it though
functions? :downloading:

Author:  CC335 [ Wed Oct 22, 2008 8:49 pm ]
Post subject:  Re: Would this code work?

This is an old topic. please don't post in it anymore.

Author:  tfriez [ Wed Oct 22, 2008 9:58 pm ]
Post subject:  Re: Would this code work?

We'll just close it. :)

Page 1 of 1 All times are UTC - 5 hours [ DST ]
Powered by phpBB © 2000, 2002, 2005, 2007 phpBB Group
http://www.phpbb.com/