top of page

Team Orion

Participating in Robocup

ABOUT US

We are the National Champions of the United States Robocup Junior lightweight league.

We are also Princeton High School students :)

Saiph - Offensive

Rigel - Offensive (seocndary)

Betelgeuse - Defensive

//--------------------------------------------------------------------------------
#include "D_Main.h"
#include "D_I2C.h"
#include "D_SIO.h"
#include "D_EIO.h"
//--------------------------------------------------------------------------------
// Functions
//--------------------------------------------------------------------------------
void user_sub_1(void);
void user_sub_4(void);
void user_main(void);
//--------------------------------------------------------------------------------
// Program Name : a new ult test 7 - leap.C
//--------------------------------------------------------------------------------
void user_sub_1(void)
{
  gV[VAR_R] = get_ping(CN7);
  gV[VAR_S] = get_ping(CN8);
  gV[VAR_T] = get_ping(CN9);
  gV[VAR_U] = get_ping(CN10);
  if (gV[VAR_S] > 1200) {
    gPwm[0] = gV[VAR_P] < 0 ? (gV[VAR_P] * -1) | 0x80 : gV[VAR_P];
    gPwm[1] = gV[VAR_P] < 0 ? gV[VAR_P] * -1 : gV[VAR_P] | 0x80;
    gPwm[2] = gV[VAR_P] < 0 ? gV[VAR_P] * -1 : gV[VAR_P] | 0x80;
    gPwm[3] = gV[VAR_P] < 0 ? (gV[VAR_P] * -1) | 0x80 : gV[VAR_P];
    gPwm[4] = 0x00;
    gPwm[5] = 0x00;
    pwm_out();
    wait_ms(200);
    gPwm[0] = 0x00 | 0x80;
    gPwm[1] = 0x00 | 0x80;
    gPwm[2] = 0x00 | 0x80;
    gPwm[3] = 0x00 | 0x80;
    gPwm[4] = 0x00;
    gPwm[5] = 0x00;
    pwm_out();
    wait_ms(200);
  } else if (gV[VAR_U] > 1200) {
    gPwm[0] = gV[VAR_P] < 0 ? gV[VAR_P] * -1 : gV[VAR_P] | 0x80;
    gPwm[1] = gV[VAR_P] < 0 ? (gV[VAR_P] * -1) | 0x80 : gV[VAR_P];
    gPwm[2] = gV[VAR_P] < 0 ? (gV[VAR_P] * -1) | 0x80 : gV[VAR_P];
    gPwm[3] = gV[VAR_P] < 0 ? gV[VAR_P] * -1 : gV[VAR_P] | 0x80;
    gPwm[4] = 0x00;
    gPwm[5] = 0x00;
    pwm_out();
    wait_ms(200);
    gPwm[0] = 0x00 | 0x80;
    gPwm[1] = 0x00 | 0x80;
    gPwm[2] = 0x00 | 0x80;
    gPwm[3] = 0x00 | 0x80;
    gPwm[4] = 0x00;
    gPwm[5] = 0x00;
    pwm_out();
    wait_ms(200);
  } else {
    if (gAD[CN5] > 10) {
      if (gV[VAR_R] > gV[VAR_T]) {
        if (gV[VAR_S] > gV[VAR_U]) {
          gPwm[0] = 0x00 | 0x80;
          gPwm[1] = gV[VAR_P] < 0 ? gV[VAR_P] * -1 : gV[VAR_P] | 0x80;
          gPwm[2] = 0x00 | 0x80;
          gPwm[3] = gV[VAR_P] < 0 ? (gV[VAR_P] * -1) | 0x80 : gV[VAR_P];
          gPwm[4] = 0x00;
          gPwm[5] = 0x00;
          pwm_out();
          wait_ms(300);
          gPwm[0] = 0x00 | 0x80;
          gPwm[1] = 0x00 | 0x80;
          gPwm[2] = 0x00 | 0x80;
          gPwm[3] = 0x00 | 0x80;
          gPwm[4] = 0x00;
          gPwm[5] = 0x00;
          pwm_out();
        } else {
          gPwm[0] = gV[VAR_P] < 0 ? gV[VAR_P] * -1 : gV[VAR_P] | 0x80;
          gPwm[1] = 0x00 | 0x80;
          gPwm[2] = gV[VAR_P] < 0 ? (gV[VAR_P] * -1) | 0x80 : gV[VAR_P];
          gPwm[3] = 0x00 | 0x80;
          gPwm[4] = 0x00;
          gPwm[5] = 0x00;
          pwm_out();
          wait_ms(300);
          gPwm[0] = 0x00 | 0x80;
          gPwm[1] = 0x00 | 0x80;
          gPwm[2] = 0x00 | 0x80;
          gPwm[3] = 0x00 | 0x80;
          gPwm[4] = 0x00;
          gPwm[5] = 0x00;
          pwm_out();
        }
      } else {
        if (gV[VAR_S] > gV[VAR_U]) {
          gPwm[0] = gV[VAR_P] < 0 ? (gV[VAR_P] * -1) | 0x80 : gV[VAR_P];
          gPwm[1] = 0x00 | 0x80;
          gPwm[2] = gV[VAR_P] < 0 ? gV[VAR_P] * -1 : gV[VAR_P] | 0x80;
          gPwm[3] = 0x00 | 0x80;
          gPwm[4] = 0x00;
          gPwm[5] = 0x00;
          pwm_out();
          wait_ms(300);
          gPwm[0] = 0x00 | 0x80;
          gPwm[1] = 0x00 | 0x80;
          gPwm[2] = 0x00 | 0x80;
          gPwm[3] = 0x00 | 0x80;
          gPwm[4] = 0x00;
          gPwm[5] = 0x00;
          pwm_out();
        } else {
          gPwm[0] = 0x00 | 0x80;
          gPwm[1] = gV[VAR_P] < 0 ? (gV[VAR_P] * -1) | 0x80 : gV[VAR_P];
          gPwm[2] = 0x00 | 0x80;
          gPwm[3] = gV[VAR_P] < 0 ? gV[VAR_P] * -1 : gV[VAR_P] | 0x80;
          gPwm[4] = 0x00;
          gPwm[5] = 0x00;
          pwm_out();
          wait_ms(300);
          gPwm[0] = 0x00 | 0x80;
          gPwm[1] = 0x00 | 0x80;
          gPwm[2] = 0x00 | 0x80;
          gPwm[3] = 0x00 | 0x80;
          gPwm[4] = 0x00;
          gPwm[5] = 0x00;
          pwm_out();
        }
      }
      wait_ms(200);
    }
  }
}
//--------------------------------------------------------------------------------
// Program Name : DirectionCorrection v8.C
//--------------------------------------------------------------------------------
void user_sub_4(void)
{
  if (judge_bno(0, gV[VAR_E], 80)) {
    while (judge_bno(0, gV[VAR_E], 80)) {
      gPwm[0] = gV[VAR_Q] < 0 ? gV[VAR_Q] * -1 : gV[VAR_Q] | 0x80;
      gPwm[1] = gV[VAR_Q] < 0 ? gV[VAR_Q] * -1 : gV[VAR_Q] | 0x80;
      gPwm[2] = gV[VAR_Q] < 0 ? gV[VAR_Q] * -1 : gV[VAR_Q] | 0x80;
      gPwm[3] = gV[VAR_Q] < 0 ? gV[VAR_Q] * -1 : gV[VAR_Q] | 0x80;
      gPwm[4] = 0x00;
      gPwm[5] = 0x00;
      pwm_out();
    }
    gPwm[0] = 0x00 | 0x80;
    gPwm[1] = 0x00 | 0x80;
    gPwm[2] = 0x00 | 0x80;
    gPwm[3] = 0x00 | 0x80;
    gPwm[4] = 0x00;
    gPwm[5] = 0x00;
    pwm_out();
  } else if (judge_bno(0, gV[VAR_F], 80)) {
    while (judge_bno(0, gV[VAR_F], 80)) {
      gPwm[0] = gV[VAR_Q] < 0 ? (gV[VAR_Q] * -1) | 0x80 : gV[VAR_Q];
      gPwm[1] = gV[VAR_Q] < 0 ? (gV[VAR_Q] * -1) | 0x80 : gV[VAR_Q];
      gPwm[2] = gV[VAR_Q] < 0 ? (gV[VAR_Q] * -1) | 0x80 : gV[VAR_Q];
      gPwm[3] = gV[VAR_Q] < 0 ? (gV[VAR_Q] * -1) | 0x80 : gV[VAR_Q];
      gPwm[4] = 0x00;
      gPwm[5] = 0x00;
      pwm_out();
    }
  }
}
//--------------------------------------------------------------------------------
// Program Name : Limter off Day 5 leap mod 23 power+.C
//--------------------------------------------------------------------------------
void user_main(void)
{
  gV[VAR_O] = 60;
  gV[VAR_P] = 40;
  gV[VAR_Q] = 10;
  gV[VAR_E] = 100;
  gV[VAR_F] = 260;
  clr_timer(0);
  while (TRUE) {
    user_sub_1();
    gV[VAR_U] = gAD[CN1];
    gV[VAR_V] = gAD[CN3];
    gV[VAR_S] = gAD[CN4];
    gV[VAR_T] = gAD[CN2] + 40;
    if (gV[VAR_U] > gV[VAR_V]) {
      if (gAD[CN1] > 818) {
        gPwm[0] = gV[VAR_P] < 0 ? gV[VAR_P] * -1 : gV[VAR_P] | 0x80;
        gPwm[1] = gV[VAR_P] < 0 ? gV[VAR_P] * -1 : gV[VAR_P] | 0x80;
        gPwm[2] = gV[VAR_P] < 0 ? (gV[VAR_P] * -1) | 0x80 : gV[VAR_P];
        gPwm[3] = gV[VAR_P] < 0 ? (gV[VAR_P] * -1) | 0x80 : gV[VAR_P];
        gPwm[4] = 0x00;
        gPwm[5] = 0x00;
        pwm_out();
        if (gAD[CN1] > 910) {
          gV[VAR_A] = get_ping(CN10);
          gV[VAR_B] = get_ping(CN8);
          if (gV[VAR_A] < gV[VAR_B]) {
            while (gAD[CN1] > 869) {
              gPwm[0] = gV[VAR_P] < 0 ? gV[VAR_P] * -1 : gV[VAR_P] | 0x80;
              gPwm[1] = gV[VAR_P] < 0 ? gV[VAR_P] * -1 : gV[VAR_P] | 0x80;
              gPwm[2] = gV[VAR_O] < 0 ? (gV[VAR_O] * -1) | 0x80 : gV[VAR_O];
              gPwm[3] = gV[VAR_O] < 0 ? (gV[VAR_O] * -1) | 0x80 : gV[VAR_O];
              pwm_out();
            }
          } else {
            while (gAD[CN1] > 869) {
              gPwm[0] = gV[VAR_O] < 0 ? gV[VAR_O] * -1 : gV[VAR_O] | 0x80;
              gPwm[1] = gV[VAR_O] < 0 ? gV[VAR_O] * -1 : gV[VAR_O] | 0x80;
              gPwm[2] = gV[VAR_P] < 0 ? (gV[VAR_P] * -1) | 0x80 : gV[VAR_P];
              gPwm[3] = gV[VAR_P] < 0 ? (gV[VAR_P] * -1) | 0x80 : gV[VAR_P];
              pwm_out();
            }
          }
        } else {
        }
      } else if (gV[VAR_U] > gV[VAR_S] + gV[VAR_T]) {
        gPwm[0] = gV[VAR_P] < 0 ? gV[VAR_P] * -1 : gV[VAR_P] | 0x80;
        gPwm[1] = gV[VAR_P] < 0 ? gV[VAR_P] * -1 : gV[VAR_P] | 0x80;
        gPwm[2] = gV[VAR_P] < 0 ? (gV[VAR_P] * -1) | 0x80 : gV[VAR_P];
        gPwm[3] = gV[VAR_P] < 0 ? (gV[VAR_P] * -1) | 0x80 : gV[VAR_P];
        gPwm[4] = 0x00;
        gPwm[5] = 0x00;
        pwm_out();
      } else if (gV[VAR_S] < gV[VAR_T]) {
        if (gV[VAR_S] > gV[VAR_T] - 40) {
          gPwm[0] = 0x00;
          gPwm[1] = gV[VAR_P] < 0 ? gV[VAR_P] * -1 : gV[VAR_P] | 0x80;
          gPwm[2] = 0x00;
          gPwm[3] = gV[VAR_P] < 0 ? (gV[VAR_P] * -1) | 0x80 : gV[VAR_P];
          gPwm[4] = 0x00;
          gPwm[5] = 0x00;
          pwm_out();
        } else {
          gPwm[0] = gV[VAR_O] < 0 ? (gV[VAR_O] * -1) | 0x80 : gV[VAR_O];
          gPwm[1] = gV[VAR_P] < 0 ? gV[VAR_P] * -1 : gV[VAR_P] | 0x80;
          gPwm[2] = gV[VAR_O] < 0 ? gV[VAR_O] * -1 : gV[VAR_O] | 0x80;
          gPwm[3] = gV[VAR_P] < 0 ? (gV[VAR_P] * -1) | 0x80 : gV[VAR_P];
          gPwm[4] = 0x00;
          gPwm[5] = 0x00;
          pwm_out();
        }
      } else {
        if (gV[VAR_T] > gV[VAR_S] - 40) {
          gPwm[0] = gV[VAR_P] < 0 ? gV[VAR_P] * -1 : gV[VAR_P] | 0x80;
          gPwm[1] = 0x00;
          gPwm[2] = gV[VAR_P] < 0 ? (gV[VAR_P] * -1) | 0x80 : gV[VAR_P];
          gPwm[3] = 0x00;
          gPwm[4] = 0x00;
          gPwm[5] = 0x00;
          pwm_out();
        } else {
          gPwm[0] = gV[VAR_P] < 0 ? gV[VAR_P] * -1 : gV[VAR_P] | 0x80;
          gPwm[1] = gV[VAR_O] < 0 ? (gV[VAR_O] * -1) | 0x80 : gV[VAR_O];
          gPwm[2] = gV[VAR_P] < 0 ? (gV[VAR_P] * -1) | 0x80 : gV[VAR_P];
          gPwm[3] = gV[VAR_O] < 0 ? gV[VAR_O] * -1 : gV[VAR_O] | 0x80;
          gPwm[4] = 0x00;
          gPwm[5] = 0x00;
          pwm_out();
        }
      }
    } else if (gAD[CN3] > 613) {
      if (gV[VAR_S] < gV[VAR_T]) {
        gPwm[0] = 0x00;
        gPwm[1] = gV[VAR_P] < 0 ? (gV[VAR_P] * -1) | 0x80 : gV[VAR_P];
        gPwm[2] = 0x00;
        gPwm[3] = gV[VAR_P] < 0 ? gV[VAR_P] * -1 : gV[VAR_P] | 0x80;
        gPwm[4] = 0x00;
        gPwm[5] = 0x00;
        pwm_out();
      } else {
        gPwm[0] = gV[VAR_P] < 0 ? (gV[VAR_P] * -1) | 0x80 : gV[VAR_P];
        gPwm[1] = 0x00;
        gPwm[2] = gV[VAR_P] < 0 ? gV[VAR_P] * -1 : gV[VAR_P] | 0x80;
        gPwm[3] = 0x00;
        gPwm[4] = 0x00;
        gPwm[5] = 0x00;
        pwm_out();
      }
    } else {
      gPwm[0] = gV[VAR_P] < 0 ? (gV[VAR_P] * -1) | 0x80 : gV[VAR_P];
      gPwm[1] = gV[VAR_P] < 0 ? (gV[VAR_P] * -1) | 0x80 : gV[VAR_P];
      gPwm[2] = gV[VAR_P] < 0 ? gV[VAR_P] * -1 : gV[VAR_P] | 0x80;
      gPwm[3] = gV[VAR_P] < 0 ? gV[VAR_P] * -1 : gV[VAR_P] | 0x80;
      gPwm[4] = 0x00;
      gPwm[5] = 0x00;
      pwm_out();
    }
    if (get_timer(T1) > 200L) {
      user_sub_4();
      clr_timer(0);
    }
  }
}
//--------------------------------------------------------------------------------

//--------------------------------------------------------------------------------
#include "D_Main.h"
#include "D_I2C.h"
#include "D_SIO.h"
#include "D_EIO.h"
//--------------------------------------------------------------------------------
// Functions
//--------------------------------------------------------------------------------
void user_sub_1(void);
void user_sub_2(void);
void user_sub_3(void);
void user_sub_8(void);
void user_sub_7(void);
void user_sub_4(void);
void user_sub_6(void);
void user_main(void);
//--------------------------------------------------------------------------------
// Program Name : Direction.C
//--------------------------------------------------------------------------------
void user_sub_1(void)
{
  gV[VAR_Q] = 50;
  gV[VAR_I] = get_hmc() - gV[VAR_D];
  if (gV[VAR_I] > 180) {
    gV[VAR_I] = gV[VAR_I] - 360;
  } else if (gV[VAR_I] < - 180) {
    gV[VAR_I] = gV[VAR_I] + 360;
  }
  if (gV[VAR_I] < 0) {
    gPwm[0] = gV[VAR_Q] < 0 ? (gV[VAR_Q] * -1) | 0x80 : gV[VAR_Q];
    gPwm[1] = gV[VAR_Q] < 0 ? (gV[VAR_Q] * -1) | 0x80 : gV[VAR_Q];
    gPwm[2] = gV[VAR_Q] < 0 ? (gV[VAR_Q] * -1) | 0x80 : gV[VAR_Q];
    gPwm[3] = gV[VAR_Q] < 0 ? gV[VAR_Q] * -1 : gV[VAR_Q] | 0x80;
    pwm_out();
  } else {
    gPwm[0] = gV[VAR_Q] < 0 ? gV[VAR_Q] * -1 : gV[VAR_Q] | 0x80;
    gPwm[1] = gV[VAR_Q] < 0 ? gV[VAR_Q] * -1 : gV[VAR_Q] | 0x80;
    gPwm[2] = gV[VAR_Q] < 0 ? gV[VAR_Q] * -1 : gV[VAR_Q] | 0x80;
    gPwm[3] = gV[VAR_Q] < 0 ? (gV[VAR_Q] * -1) | 0x80 : gV[VAR_Q];
    pwm_out();
  }
  if (judge_hmc(0, 15)) {
  }
  gPwm[0] = 0x00 | 0x80;
  gPwm[1] = 0x00 | 0x80;
  gPwm[2] = 0x00 | 0x80;
  gPwm[3] = 0x00 | 0x80;
  pwm_out();
}
//--------------------------------------------------------------------------------
// Program Name : MoveForwards.C
//--------------------------------------------------------------------------------
void user_sub_2(void)
{
  gV[VAR_P] = 100;
  gPwm[0] = gV[VAR_P] < 0 ? gV[VAR_P] * -1 : gV[VAR_P] | 0x80;
  gPwm[1] = gV[VAR_P] < 0 ? gV[VAR_P] * -1 : gV[VAR_P] | 0x80;
  gPwm[2] = gV[VAR_P] < 0 ? (gV[VAR_P] * -1) | 0x80 : gV[VAR_P];
  gPwm[3] = gV[VAR_P] < 0 ? gV[VAR_P] * -1 : gV[VAR_P] | 0x80;
  pwm_out();
}
//--------------------------------------------------------------------------------
// Program Name : MoveBackwards.C
//--------------------------------------------------------------------------------
void user_sub_3(void)
{
  gV[VAR_P] = 100;
  gPwm[0] = gV[VAR_P] < 0 ? (gV[VAR_P] * -1) | 0x80 : gV[VAR_P];
  gPwm[1] = gV[VAR_P] < 0 ? (gV[VAR_P] * -1) | 0x80 : gV[VAR_P];
  gPwm[2] = gV[VAR_P] < 0 ? gV[VAR_P] * -1 : gV[VAR_P] | 0x80;
  gPwm[3] = gV[VAR_P] < 0 ? (gV[VAR_P] * -1) | 0x80 : gV[VAR_P];
  pwm_out();
}
//--------------------------------------------------------------------------------
// Program Name : RightForwards.C
//--------------------------------------------------------------------------------
void user_sub_8(void)
{
  gV[VAR_P] = 100;
  gPwm[0] = 0x00;
  gPwm[1] = gV[VAR_P] < 0 ? gV[VAR_P] * -1 : gV[VAR_P] | 0x80;
  gPwm[2] = 0x00;
  gPwm[3] = gV[VAR_P] < 0 ? gV[VAR_P] * -1 : gV[VAR_P] | 0x80;
  pwm_out();
}
//--------------------------------------------------------------------------------
// Program Name : Left Backwards.C
//--------------------------------------------------------------------------------
void user_sub_7(void)
{
  gV[VAR_P] = 100;
  gPwm[0] = 0x00;
  gPwm[1] = gV[VAR_P] < 0 ? (gV[VAR_P] * -1) | 0x80 : gV[VAR_P];
  gPwm[2] = 0x00;
  gPwm[3] = gV[VAR_P] < 0 ? (gV[VAR_P] * -1) | 0x80 : gV[VAR_P];
  pwm_out();
}
//--------------------------------------------------------------------------------
// Program Name : LeftForwards.C
//--------------------------------------------------------------------------------
void user_sub_4(void)
{
  gV[VAR_P] = 100;
  gPwm[0] = gV[VAR_P] < 0 ? gV[VAR_P] * -1 : gV[VAR_P] | 0x80;
  gPwm[1] = 0x00;
  gPwm[2] = gV[VAR_P] < 0 ? (gV[VAR_P] * -1) | 0x80 : gV[VAR_P];
  gPwm[3] = 0x00;
  pwm_out();
}
//--------------------------------------------------------------------------------
// Program Name : Right Backwards.C
//--------------------------------------------------------------------------------
void user_sub_6(void)
{
  gV[VAR_P] = 100;
  gPwm[0] = gV[VAR_P] < 0 ? (gV[VAR_P] * -1) | 0x80 : gV[VAR_P];
  gPwm[1] = 0x00;
  gPwm[2] = gV[VAR_P] < 0 ? gV[VAR_P] * -1 : gV[VAR_P] | 0x80;
  gPwm[3] = 0x00;
  pwm_out();
}
//--------------------------------------------------------------------------------
// Program Name : Yota Soccer Program.C
//--------------------------------------------------------------------------------
void user_main(void)
{
  clr_timer(0);
  gV[VAR_D] = get_hmc();
  while (TRUE) {
    if (judge_hmc(0, 15)) {
    } else {
      user_sub_1();
      continue;
    }
    gV[VAR_F] = gAD[CN2];
    gV[VAR_R] = gAD[CN3] + 300;
    gV[VAR_L] = gAD[CN5];
    if (gV[VAR_F] > 850) {
      user_sub_2();
      if (gAD[CN1] > 102) {
        user_sub_3();
        wait_ms(300);
        continue;
      } else if (gAD[CN1] > 102) {
        user_sub_3();
        wait_ms(300);
        continue;
      } else if (gAD[CN4] > 102) {
        user_sub_3();
        wait_ms(300);
        continue;
      } else if (gAD[CN6] > 102) {
        user_sub_3();
        wait_ms(300);
        continue;
      }
      continue;
    } else {
      if (gV[VAR_F] > 700) {
        if (gV[VAR_R] > gV[VAR_L]) {
          user_sub_8();
          if (gAD[CN1] > 102) {
            user_sub_7();
            wait_ms(300);
            if (gV[VAR_R] > gV[VAR_L]) {
              gPwm[0] = 0x00 | 0x80;
              gPwm[1] = 0x00 | 0x80;
              gPwm[2] = 0x00 | 0x80;
              gPwm[3] = 0x00 | 0x80;
              pwm_out();
            } else {
              continue;
            }
          } else if (gAD[CN4] > 102) {
            user_sub_7();
            wait_ms(300);
            if (gV[VAR_R] > gV[VAR_L]) {
              gPwm[0] = 0x00 | 0x80;
              gPwm[1] = 0x00 | 0x80;
              gPwm[2] = 0x00 | 0x80;
              gPwm[3] = 0x00 | 0x80;
              pwm_out();
            } else {
              continue;
            }
          } else if (gAD[CN6] > 102) {
            user_sub_7();
            wait_ms(300);
            if (gV[VAR_R] > gV[VAR_L]) {
              gPwm[0] = 0x00 | 0x80;
              gPwm[1] = 0x00 | 0x80;
              gPwm[2] = 0x00 | 0x80;
              gPwm[3] = 0x00 | 0x80;
              pwm_out();
            } else {
              continue;
            }
          }
          continue;
        } else if (gV[VAR_L] > gV[VAR_R]) {
          user_sub_4();
          if (gAD[CN1] > 102) {
            user_sub_6();
            wait_ms(300);
            if (gV[VAR_R] < gV[VAR_L]) {
              gPwm[0] = 0x00 | 0x80;
              gPwm[1] = 0x00 | 0x80;
              gPwm[2] = 0x00 | 0x80;
              gPwm[3] = 0x00 | 0x80;
              pwm_out();
            } else {
              continue;
            }
          } else if (gAD[CN4] > 102) {
            user_sub_6();
            wait_ms(300);
            if (gV[VAR_R] < gV[VAR_L]) {
              gPwm[0] = 0x00 | 0x80;
              gPwm[1] = 0x00 | 0x80;
              gPwm[2] = 0x00 | 0x80;
              gPwm[3] = 0x00 | 0x80;
              pwm_out();
            } else {
              continue;
            }
          } else if (gAD[CN6] > 102) {
            user_sub_6();
            wait_ms(300);
            if (gV[VAR_R] < gV[VAR_L]) {
              gPwm[0] = 0x00 | 0x80;
              gPwm[1] = 0x00 | 0x80;
              gPwm[2] = 0x00 | 0x80;
              gPwm[3] = 0x00 | 0x80;
              pwm_out();
            } else {
              continue;
            }
          }
        }
      } else {
        if (gV[VAR_R] > gV[VAR_L]) {
          user_sub_6();
          if (gAD[CN1] > 102) {
            user_sub_7();
            wait_ms(300);
            if (gV[VAR_R] > gV[VAR_L]) {
              gPwm[0] = 0x00 | 0x80;
              gPwm[1] = 0x00 | 0x80;
              gPwm[2] = 0x00 | 0x80;
              gPwm[3] = 0x00 | 0x80;
              pwm_out();
            } else {
              continue;
            }
          } else if (gAD[CN4] > 102) {
            user_sub_7();
            wait_ms(300);
            if (gV[VAR_R] > gV[VAR_L]) {
              gPwm[0] = 0x00 | 0x80;
              gPwm[1] = 0x00 | 0x80;
              gPwm[2] = 0x00 | 0x80;
              gPwm[3] = 0x00 | 0x80;
              pwm_out();
            } else {
              continue;
            }
          } else if (gAD[CN6] > 102) {
            user_sub_7();
            wait_ms(300);
            if (gV[VAR_R] > gV[VAR_L]) {
              gPwm[0] = 0x00 | 0x80;
              gPwm[1] = 0x00 | 0x80;
              gPwm[2] = 0x00 | 0x80;
              gPwm[3] = 0x00 | 0x80;
              pwm_out();
            } else {
              continue;
            }
          }
        } else if (gV[VAR_L] > gV[VAR_R]) {
          user_sub_7();
          if (gAD[CN1] > 102) {
            user_sub_6();
            wait_ms(300);
            if (gV[VAR_R] < gV[VAR_L]) {
              gPwm[0] = 0x00 | 0x80;
              gPwm[1] = 0x00 | 0x80;
              gPwm[2] = 0x00 | 0x80;
              gPwm[3] = 0x00 | 0x80;
              pwm_out();
            } else {
              continue;
            }
          } else if (gAD[CN4] > 102) {
            user_sub_6();
            wait_ms(300);
            if (gV[VAR_R] < gV[VAR_L]) {
              gPwm[0] = 0x00 | 0x80;
              gPwm[1] = 0x00 | 0x80;
              gPwm[2] = 0x00 | 0x80;
              gPwm[3] = 0x00 | 0x80;
              pwm_out();
            } else {
              continue;
            }
          } else if (gAD[CN6] > 102) {
            user_sub_6();
            wait_ms(300);
            if (gV[VAR_R] < gV[VAR_L]) {
              gPwm[0] = 0x00 | 0x80;
              gPwm[1] = 0x00 | 0x80;
              gPwm[2] = 0x00 | 0x80;
              gPwm[3] = 0x00 | 0x80;
              pwm_out();
            } else {
              continue;
            }
          }
        }
      }
    }
  }
}
//--------------------------------------------------------------------------------

//--------------------------------------------------------------------------------
#include "D_Main.h"
#include "D_I2C.h"
#include "D_SIO.h"
#include "D_EIO.h"
//--------------------------------------------------------------------------------
// Functions
//--------------------------------------------------------------------------------
void user_sub_1(void);
void user_sub_3(void);
void user_sub_2(void);
void user_sub_4(void);
void user_sub_5(void);
void user_sub_10(void);
void user_sub_11(void);
void user_sub_6(void);
void user_sub_7(void);
void user_main(void);
//--------------------------------------------------------------------------------
// Program Name : setVariables.C
//--------------------------------------------------------------------------------
void user_sub_1(void)
{
    gV[VAR_F] = gAD[CN1];
    gV[VAR_R] = gAD[CN2];
    gV[VAR_L] = gAD[CN3];
    gV[VAR_B] = gAD[CN4];
    gV[VAR_W] = 0;
    gV[VAR_X] = get_ping(CN10);
    gV[VAR_Y] = get_ping(CN9);
    gV[VAR_Z] = 0;
    gV[VAR_A] = gV[VAR_F] + gV[VAR_B];
    gV[VAR_A] = gV[VAR_A] + gV[VAR_R];
    gV[VAR_A] = gV[VAR_A] + gV[VAR_L];
    gV[VAR_D] = get_bno(0);
    if (gV[VAR_D] > 180) {
        gV[VAR_D] = gV[VAR_D] - 360;
    } else {
        gV[VAR_D] = gV[VAR_D];
    }
    gV[VAR_D] = gV[VAR_I] - gV[VAR_D];
}
//--------------------------------------------------------------------------------
// Program Name : clockwise.C
//--------------------------------------------------------------------------------
void user_sub_3(void)
{
    gPwm[0] = gV[VAR_T] < 0 ? gV[VAR_T] * -1 : gV[VAR_T] | 0x80;
    gPwm[1] = gV[VAR_T] < 0 ? (gV[VAR_T] * -1) | 0x80 : gV[VAR_T];
    gPwm[2] = gV[VAR_T] < 0 ? gV[VAR_T] * -1 : gV[VAR_T] | 0x80;
    gPwm[3] = gV[VAR_T] < 0 ? (gV[VAR_T] * -1) | 0x80 : gV[VAR_T];
    pwm_out();
}
//--------------------------------------------------------------------------------
// Program Name : counterclockwise.C
//--------------------------------------------------------------------------------
void user_sub_2(void)
{
    gPwm[0] = gV[VAR_T] < 0 ? (gV[VAR_T] * -1) | 0x80 : gV[VAR_T];
    gPwm[1] = gV[VAR_T] < 0 ? gV[VAR_T] * -1 : gV[VAR_T] | 0x80;
    gPwm[2] = gV[VAR_T] < 0 ? (gV[VAR_T] * -1) | 0x80 : gV[VAR_T];
    gPwm[3] = gV[VAR_T] < 0 ? gV[VAR_T] * -1 : gV[VAR_T] | 0x80;
    pwm_out();
}
//--------------------------------------------------------------------------------
// Program Name : left.C
//--------------------------------------------------------------------------------
void user_sub_4(void)
{
    if (gAD[CN5] < 30) {
        if (gV[VAR_W] < 1000) {
            if (gV[VAR_Y] > gV[VAR_X] + 100) {
                gPwm[0] = 0x00 | 0x80;
                gPwm[1] = 0x00 | 0x80;
                gPwm[2] = 0x00 | 0x80;
                gPwm[3] = 0x00 | 0x80;
                pwm_out();
            } else {
                gPwm[0] = gV[VAR_P] < 0 ? (gV[VAR_P] * -1) | 0x80 : gV[VAR_P];
                gPwm[1] = gV[VAR_P] < 0 ? gV[VAR_P] * -1 : gV[VAR_P] | 0x80;
                gPwm[2] = gV[VAR_P] < 0 ? gV[VAR_P] * -1 : gV[VAR_P] | 0x80;
                gPwm[3] = gV[VAR_P] < 0 ? (gV[VAR_P] * -1) | 0x80 : gV[VAR_P];
                pwm_out();
            }
        } else {
            gPwm[0] = 0x00 | 0x80;
            gPwm[1] = 0x00 | 0x80;
            gPwm[2] = 0x00 | 0x80;
            gPwm[3] = 0x00 | 0x80;
            pwm_out();
        }
    } else {
        gPwm[0] = gV[VAR_P] < 0 ? gV[VAR_P] * -1 : gV[VAR_P] | 0x80;
        gPwm[1] = gV[VAR_P] < 0 ? (gV[VAR_P] * -1) | 0x80 : gV[VAR_P];
        gPwm[2] = gV[VAR_P] < 0 ? (gV[VAR_P] * -1) | 0x80 : gV[VAR_P];
        gPwm[3] = gV[VAR_P] < 0 ? gV[VAR_P] * -1 : gV[VAR_P] | 0x80;
        pwm_out();
        while (get_Uss(USS4) < 500) {
        }
        gPwm[0] = 0x00 | 0x80;
        gPwm[1] = 0x00 | 0x80;
        gPwm[2] = 0x00 | 0x80;
        gPwm[3] = 0x00 | 0x80;
        pwm_out();
    }
}
//--------------------------------------------------------------------------------
// Program Name : right.C
//--------------------------------------------------------------------------------
void user_sub_5(void)
{
    if (gAD[CN5] < 30) {
        if (gV[VAR_Z] < 1000) {
            if (gV[VAR_X] > gV[VAR_Y] + 100) {
                gPwm[0] = 0x00 | 0x80;
                gPwm[1] = 0x00 | 0x80;
                gPwm[2] = 0x00 | 0x80;
                gPwm[3] = 0x00 | 0x80;
                pwm_out();
            } else {
                gPwm[0] = gV[VAR_P] < 0 ? gV[VAR_P] * -1 : gV[VAR_P] | 0x80;
                gPwm[1] = gV[VAR_P] < 0 ? (gV[VAR_P] * -1) | 0x80 : gV[VAR_P];
                gPwm[2] = gV[VAR_P] < 0 ? (gV[VAR_P] * -1) | 0x80 : gV[VAR_P];
                gPwm[3] = gV[VAR_P] < 0 ? gV[VAR_P] * -1 : gV[VAR_P] | 0x80;
                pwm_out();
            }
        } else {
            gPwm[0] = 0x00 | 0x80;
            gPwm[1] = 0x00 | 0x80;
            gPwm[2] = 0x00 | 0x80;
            gPwm[3] = 0x00 | 0x80;
            pwm_out();
        }
    } else {
        gPwm[0] = gV[VAR_P] < 0 ? (gV[VAR_P] * -1) | 0x80 : gV[VAR_P];
        gPwm[1] = gV[VAR_P] < 0 ? gV[VAR_P] * -1 : gV[VAR_P] | 0x80;
        gPwm[2] = gV[VAR_P] < 0 ? gV[VAR_P] * -1 : gV[VAR_P] | 0x80;
        gPwm[3] = gV[VAR_P] < 0 ? (gV[VAR_P] * -1) | 0x80 : gV[VAR_P];
        pwm_out();
        while (get_Uss(USS1) < 500) {
        }
        gPwm[0] = 0x00 | 0x80;
        gPwm[1] = 0x00 | 0x80;
        gPwm[2] = 0x00 | 0x80;
        gPwm[3] = 0x00 | 0x80;
        pwm_out();
    }
}
//--------------------------------------------------------------------------------
// Program Name : backward-left.C
//--------------------------------------------------------------------------------
void user_sub_10(void)
{
    if (gAD[CN5] < 30) {
        if (get_Uss(USS1) < 1100) {
            gPwm[0] = gV[VAR_P] < 0 ? (gV[VAR_P] * -1) | 0x80 : gV[VAR_P];
            gPwm[1] = 0x00;
            gPwm[2] = 0x00;
            gPwm[3] = gV[VAR_P] < 0 ? (gV[VAR_P] * -1) | 0x80 : gV[VAR_P];
            pwm_out();
        } else {
            gPwm[0] = 0x00 | 0x80;
            gPwm[1] = 0x00 | 0x80;
            gPwm[2] = 0x00 | 0x80;
            gPwm[3] = 0x00 | 0x80;
            pwm_out();
        }
    } else {
        gPwm[0] = gV[VAR_P] < 0 ? gV[VAR_P] * -1 : gV[VAR_P] | 0x80;
        gPwm[1] = 0x00;
        gPwm[2] = 0x00;
        gPwm[3] = gV[VAR_P] < 0 ? gV[VAR_P] * -1 : gV[VAR_P] | 0x80;
        pwm_out();
        wait_ms(200);
    }
}
//--------------------------------------------------------------------------------
// Program Name : backward-right.C
//--------------------------------------------------------------------------------
void user_sub_11(void)
{
    if (gAD[CN5] < 30) {
        if (get_Uss(USS4) < 1100) {
            gPwm[0] = 0x00;
            gPwm[1] = gV[VAR_P] < 0 ? (gV[VAR_P] * -1) | 0x80 : gV[VAR_P];
            gPwm[2] = gV[VAR_P] < 0 ? (gV[VAR_P] * -1) | 0x80 : gV[VAR_P];
            gPwm[3] = 0x00;
            pwm_out();
        } else {
            gPwm[0] = 0x00 | 0x80;
            gPwm[1] = 0x00 | 0x80;
            gPwm[2] = 0x00 | 0x80;
            gPwm[3] = 0x00 | 0x80;
            pwm_out();
        }
    } else {
        gPwm[0] = 0x00;
        gPwm[1] = gV[VAR_P] < 0 ? gV[VAR_P] * -1 : gV[VAR_P] | 0x80;
        gPwm[2] = gV[VAR_P] < 0 ? gV[VAR_P] * -1 : gV[VAR_P] | 0x80;
        gPwm[3] = 0x00;
        pwm_out();
        wait_ms(200);
    }
}
//--------------------------------------------------------------------------------
// Program Name : backwards.C
//--------------------------------------------------------------------------------
void user_sub_6(void)
{
    if (gAD[CN5] < 30) {
        if (gV[VAR_X] > 120) {
            gPwm[0] = gV[VAR_P] < 0 ? (gV[VAR_P] * -1) | 0x80 : gV[VAR_P];
            gPwm[1] = gV[VAR_P] < 0 ? (gV[VAR_P] * -1) | 0x80 : gV[VAR_P];
            gPwm[2] = gV[VAR_P] < 0 ? (gV[VAR_P] * -1) | 0x80 : gV[VAR_P];
            gPwm[3] = gV[VAR_P] < 0 ? (gV[VAR_P] * -1) | 0x80 : gV[VAR_P];
            pwm_out();
        } else if (gV[VAR_Y] > 120) {
            gPwm[0] = gV[VAR_P] < 0 ? (gV[VAR_P] * -1) | 0x80 : gV[VAR_P];
            gPwm[1] = gV[VAR_P] < 0 ? (gV[VAR_P] * -1) | 0x80 : gV[VAR_P];
            gPwm[2] = gV[VAR_P] < 0 ? (gV[VAR_P] * -1) | 0x80 : gV[VAR_P];
            gPwm[3] = gV[VAR_P] < 0 ? (gV[VAR_P] * -1) | 0x80 : gV[VAR_P];
            pwm_out();
        } else {
            gPwm[0] = 0x00 | 0x80;
            gPwm[1] = 0x00 | 0x80;
            gPwm[2] = 0x00 | 0x80;
            gPwm[3] = 0x00 | 0x80;
            pwm_out();
        }
    } else {
        gPwm[0] = gV[VAR_P] < 0 ? gV[VAR_P] * -1 : gV[VAR_P] | 0x80;
        gPwm[1] = gV[VAR_P] < 0 ? gV[VAR_P] * -1 : gV[VAR_P] | 0x80;
        gPwm[2] = gV[VAR_P] < 0 ? gV[VAR_P] * -1 : gV[VAR_P] | 0x80;
        gPwm[3] = gV[VAR_P] < 0 ? gV[VAR_P] * -1 : gV[VAR_P] | 0x80;
        pwm_out();
        wait_ms(1000);
    }
}
//--------------------------------------------------------------------------------
// Program Name : forward.C
//--------------------------------------------------------------------------------
void user_sub_7(void)
{
    if (gAD[CN5] < 30) {
        gPwm[0] = gV[VAR_P] < 0 ? gV[VAR_P] * -1 : gV[VAR_P] | 0x80;
        gPwm[1] = gV[VAR_P] < 0 ? gV[VAR_P] * -1 : gV[VAR_P] | 0x80;
        gPwm[2] = gV[VAR_P] < 0 ? gV[VAR_P] * -1 : gV[VAR_P] | 0x80;
        gPwm[3] = gV[VAR_P] < 0 ? gV[VAR_P] * -1 : gV[VAR_P] | 0x80;
        pwm_out();
    } else {
        gPwm[0] = gV[VAR_P] < 0 ? (gV[VAR_P] * -1) | 0x80 : gV[VAR_P];
        gPwm[1] = gV[VAR_P] < 0 ? (gV[VAR_P] * -1) | 0x80 : gV[VAR_P];
        gPwm[2] = gV[VAR_P] < 0 ? (gV[VAR_P] * -1) | 0x80 : gV[VAR_P];
        gPwm[3] = gV[VAR_P] < 0 ? (gV[VAR_P] * -1) | 0x80 : gV[VAR_P];
        pwm_out();
        wait_ms(500);
    }
}
//--------------------------------------------------------------------------------
// Program Name : simple-keeper.C
//--------------------------------------------------------------------------------
void user_main(void)
{
    gV[VAR_T] = 10;
    gV[VAR_I] = get_bno(0);
    if (gV[VAR_I] > 180) {
        gV[VAR_I] = gV[VAR_I] - 360;
    } else {
        gV[VAR_I] = gV[VAR_I];
    }
    while (TRUE) {
        gV[VAR_P] = 30;
        user_sub_1();
        if (gV[VAR_F] < 90) {
            clr_timer(0);
        } else {
        }
        if (gV[VAR_D] > 10) {
            user_sub_3();
        } else if (gV[VAR_D] < -10) {
            user_sub_2();
        } else {
            if (gV[VAR_W] > 1200) {
                gPwm[0] = 0x00 | 0x80;
                gPwm[1] = 0x00 | 0x80;
                gPwm[2] = 0x00 | 0x80;
                gPwm[3] = 0x00 | 0x80;
                pwm_out();
            } else if (gV[VAR_Z] > 1200) {
                gPwm[0] = 0x00 | 0x80;
                gPwm[1] = 0x00 | 0x80;
                gPwm[2] = 0x00 | 0x80;
                gPwm[3] = 0x00 | 0x80;
                pwm_out();
            } else if (gV[VAR_X] > gV[VAR_Y] + 100) {
                user_sub_4();
            } else if (gV[VAR_Y] > gV[VAR_X] + 100) {
                user_sub_5();
            } else if (gV[VAR_X] > 250) {
                if (gAD[CN4] > 613) {
                    if (gV[VAR_R] > gV[VAR_L]) {
                        user_sub_10();
                    } else {
                        user_sub_11();
                    }
                } else {
                    user_sub_6();
                }
            } else if (gV[VAR_Y] > 250) {
                if (gAD[CN4] > 613) {
                    if (gV[VAR_R] > gV[VAR_L]) {
                        user_sub_10();
                    } else {
                        user_sub_11();
                    }
                } else {
                    user_sub_6();
                }
            } else {
                gV[VAR_P] = 40;
                if (gV[VAR_A] == 0) {
                    gPwm[0] = 0x00 | 0x80;
                    gPwm[1] = 0x00 | 0x80;
                    gPwm[2] = 0x00 | 0x80;
                    gPwm[3] = 0x00 | 0x80;
                    pwm_out();
                } else if (gV[VAR_R] > gV[VAR_L] + 10) {
                    user_sub_5();
                } else if (gV[VAR_L] > gV[VAR_R] + 10) {
                    user_sub_4();
                } else {
                    if (get_timer(T1) > 3000L) {
                        gV[VAR_P] = 40;
                        while (gV[VAR_F] > gV[VAR_B]) {
                            user_sub_1();
                            if (gV[VAR_D] > 10) {
                                user_sub_3();
                            } else if (gV[VAR_D] < -10) {
                                user_sub_2();
                            } else {
                                if (gV[VAR_F] > 900) {
                                    user_sub_7();
                                } else if (gV[VAR_R] > gV[VAR_L]) {
                                    user_sub_5();
                                } else if (gV[VAR_L] > gV[VAR_R]) {
                                    user_sub_4();
                                } else {
                                    user_sub_7();
                                }
                            }
                        }
                    } else {
                        gPwm[0] = 0x00 | 0x80;
                        gPwm[1] = 0x00 | 0x80;
                        gPwm[2] = 0x00 | 0x80;
                        gPwm[3] = 0x00 | 0x80;
                        pwm_out();
                    }
                }
            }
        }
    }
}
//--------------------------------------------------------------------------------

About

Join us!

We have just started last year and we are looking for new members! Please come to our workshop!

Next Year
Princeton Team logo
Robocup Junior
Soccer Robots
Example of a match
Community
Spaces

RoboCup and Princeton Team

Membership

Workshops

Our robocup workshops

We meet about once every 2 weeks

Other members will be there!

Get awesome ideas from other members!

The workshops are held at Tiger Labs

A Ping Pong table when you want to take a break!

People of all skill levels are welcome

Contact

CONTACT US

Workshop Address: 252 Nassau St, Princeton, NJ 08542

Come to one of our workshops!

The Princeton Team

bottom of page