/**************************************************************************

Main.c

ME 218B Project Hats Off

Team Firefox

March 7, 2010

 

***************************************************************************/

 

/*-------------------------- Include Files -----------------------------*/

#include <stdio.h>

#include <termio.h>

 

#include <hidef.h>

#include <mc9s12e128.h>

#include <bitdefs.h>

#include <S12E128bits.h>

#include "S12eVec.h"

 

#include "ADS12.h"

#include "ME218_E128.h"

#include "PWM01.h"

#include "Motors.h"

#include "myTimer.h"

#include "InputsModule.h"

#include "TapeSensors.h"

#include "Position.h"

#include "Arm.h"

#include "SPI.h"

 

//included with main.c for E128

/* common defines and macros */

/* derivative information */

#pragma LINK_INFO DERIVATIVE "SampleS12"

#define printf (void)printf

 

typedef enum{     TOTARGET,

                  HITTING,

                  CONFIRMING_HIT,

                  GETTING_NEW_TARGET,

                  TOHOME,

                  WAVING,

                  DONE,

                  ERROR_OUT } MainState_t;

 

/*-------------------------- Module Functions -----------------------------*/

void readIR(void);

void setLED(char turnOn);

/*-------------------------- Module Variables -----------------------------*/

static char IR_Aligned = FALSE;

static POS_StartColor_t startColor;

static SPI_Status_t SPIstatus;

static MainState_t state, lastState;

static POS_State_t pos_state;

static char ArmStopped;

 

static int i=0, pos = 0, cmd = 0, value=0;;

static char ch, resetPosFlag, blinkState = 0;

static int curTarget, lastTarget;

static unsigned int lastPrint = 0, lastRead = 0, swingTime = 0, lastBumpTime;

 

char waitingOnBump, waveCount;

 

/*-------------------------- Module Cal Variables -------------------------*/

const static int targetDestroyedTimeout = 5000; //in [ms]

const static char numWave = 2;

 

void main(){

  (void)printf("Initializing Timer . . .\r\n");

  MyTimer_Init();             //Initialize 1 [ms] timer for general use

  wait(500);                  //allow circuits to power up before running init code

  (void)printf("\nProgram Started\r\n");

  //Set output pins

  DDRU = 0x03;                //00000011 set U pins. 1=output

  DDRP = 0x0F;                //00001111

  DDRT = 0x8E;                //10001110  set T pins

  DDRS = 0x0C;                //00001100

  (void)ADS12_Init("AAAAAAAA");

 

  (void)printf("Initializing Arm . . .\r\n");

  Arm_Init();                //Initialize Arm module

 

  (void)printf("Initializing Motors . . .\r\n");

  Motors_Init();             //Initialize DC Motor module

 

 

  (void)printf("Initializing SPI . . .\r\n");

  SPI_Init();                //will wait until we get command from overlord

 

  //determine which team robot is on based on first assigned beacon

  curTarget = SPI_getCurTarget();

  if (curTarget == 0) {

      startColor = POS_RED;   //red team!

      PTT |= BIT1HI;          //turn on red LED

      PTT &= BIT3LO;          //turn off green LED

  }

  else if (curTarget == 8) {

      startColor = POS_GREEN; //green team!

      PTT |= BIT3HI;           //turn on green LED

      PTT &= BIT1LO;           //turn off red LED

  }

  else {

      //not assigned target 0 or 8 first, ERROR!!

      //turn on both red and green LEDs

      PTT |= BIT1HI;          //turn on red LED

      PTT |= BIT3HI;          //turn on green LED

      (void)printf("ERROR!! Not assigned target 0 or 8 to start\r\n");

      while(1) {};            //do nothing/soft crash

  }

 

  InitPositioning(startColor);  //Initialize positioning

  SPI_requestTargetStatus();    //check if current beacon was already knocked off

  (void)printf("Playing the Game . . .\r\n");

  lastPrint = MyTimer_GetTime();

 

  //Move to Start Position (target 0 or 8)

  (void)Pos_GoToStart();

  goToAttackPosition();      //move arm to attack position

  state = TOTARGET;

  lastState = ERROR;

  resetPosFlag = FALSE;

  waitingOnBump = FALSE;

 

  //game on

  while (1) {

    SPIstatus = SPI_getStatus();            //always communicate with Overlord

    ArmStopped = isArmStopped();            //always check arm status

      pos_state = Pos_getStatus(SPIstatus); //always check positioning state

 

      if (state != lastState) {

            (void)printf("Main State: ");

            switch(state) {

                  case TOTARGET: (void)printf("Moving to Target #%d\r\n", curTarget); break;

                  case HITTING: (void)printf("Hitting Target\r\n"); break;

                  case CONFIRMING_HIT: (void)printf("Confirming Target Destroyed\r\n"); break;

                  case GETTING_NEW_TARGET: (void)printf("Waiting for New Target\r\n"); break;

                  case TOHOME: (void)printf("Heading Home\r\n"); break;

                  case DONE: (void)printf("All done, game over\r\n"); break;

                  case ERROR_OUT: (void)printf("\r\nERRORED OUT\r\n"); break;

            }

            lastState = state;

      }

      switch (state) {

            case TOTARGET:      //currently moving to target

                  if ((pos_state == POS_STOPPED) && (SPIstatus != SPI_DELAY)) {   //in position and not delayed

                        if (SPIstatus == SPI_GAME_OVER) {      //check if game over, then abort hit

                          (void)Pos_GoToHome();

                          state = TOHOME;

                        }

                        else {

                          swingArm(curTarget);

                          state = HITTING;

                        }

                  }

                  break;

            case HITTING:    //currently striking the target

              if (ArmStopped) {

                state = CONFIRMING_HIT;

                SPI_requestTargetStatus();

              }

                  break;

            case CONFIRMING_HIT:    //arms has completed swing, checking if target was destroyed

              if (SPIstatus == SPI_GAME_OVER) {   //check if game over

                    (void)Pos_GoToHome();

                    state = TOHOME;

                    break;

              }

              if (SPIstatus == SPI_TARGET_STATUS_READY) {   //if delayed, this should never be true

                  if (SPI_isCurTargetHit()) {

                    state = GETTING_NEW_TARGET;             //target destroyed

                    lastTarget = curTarget;

                    resetPosFlag = FALSE;

                  }

                  else {     //target wasn't hit

                    if (resetPosFlag) {                    //already reset position, skip current target

                      resetPosFlag = FALSE;

                      SPI_skipCurTarget();

                              state = GETTING_NEW_TARGET;

                              lastTarget = curTarget;

                    }

                    else {   //need to reset our position using tape

                          if (Pos_ResetCurrentPos(curTarget) == FALSE) (void)printf("ERROR: Failed to reset position\r\n");

                          state = TOTARGET;

                          resetPosFlag = TRUE;

                    }

                  }

              }

                  break;

            case GETTING_NEW_TARGET:                //waiting to be assigned a new target

                  if (SPIstatus == SPI_GAME_OVER) { //if game over, go home

                    (void)printf("sent bot home from GETTING_NEW_TARGET with this set of procs:\r\n");

                    //send bot home

                    if (Pos_GoToHome() == FALSE) (void)printf("ERROR: Failed to go home\r\n");

                    debug_printCurExecPlan();     //print planned path out to screeen

                    state = TOHOME;

                    break;

                  }

                  curTarget = SPI_getCurTarget(); //get new target from SPI module

                  if (curTarget != lastTarget) {

                    printf("Getting New Target, newTarget = %d\r\n", curTarget);

                    lastTarget = curTarget;

                  }

                  if (curTarget != -1) {

                        (void)Pos_GoToTarget(curTarget); //move to new target

                        state = TOTARGET;

                  }

                  break;

            case TOHOME:                          //moving home

                  if (pos_state == POS_STOPPED) { //in position

                        //start doing the wave

                        waveArm();

                        waveCount = 0;

                        state = WAVING;

                  }

                  break;

        case WAVING:                       //at home, currently doing victory wave

          if (isWaveStopped()) {

            waveCount++;

            if (waveCount < numWave) {

              waveArm();                   //wave arm again

            }

            else {

              goToAttackPosition();        //return arm to centered position

              state = DONE;

            }

          }

          break;

            case DONE:               //all done, just sit here

              if (isArmStopped()) {

                 goToHomePosition(); //make arm go back to rest position on top of robot

              }

                  //do nothing

                  break;

            case ERROR_OUT:          //something went wrong

            default:

                  //do nothing for now

                  break;

      }

  }

}