/********************************************************
*                                                       *
* Title:       demotwo.c                                *
* Programmer:  Andrew Sinclair                          *
* Date:        27 July 2000                             *
* Version:     1.0                                      *
*                                                       *
* Description:                                          *
* A program for the MMPP robot to fire rocket engines.  *
*                                                       *
********************************************************/


/******************** Includes *************************/
#include <analog.h>
#include <motorme.h>
#include <servome.h>
#include <clocktjp.h>
#include <isrdecl.h>
#include <vectors.h>
#include <stdio.h>
/***************** End of Includes *********************/


/******************** Constants ************************/
#define BUMPER analog(3)
/**************** End of Constants *********************/

void main(void)
/********************** Main ***************************/
{
 char mirror, mask;
 	
 init_analog();
 init_clocktjp();
	
 *(unsigned char *) 0x7000=0x00; /*Turn off outputs*/
 mirror=0x00; /*Initialize mirror to same as outputs*/
 	
 /*Load rockets*/
 /*Accelerate and then brake*/ 	
 /*******************************************/
 while(BUMPER<120); /*Press the bumper to continue the program*/
 wait(10000); /*Wait time in ms*/
 
 /*Acceleration*/
 /*First Firing - bit 6*/
 mask=0x40;
 mirror=mirror|mask;
 *(unsigned char *) 0x7000=mirror;

 wait(1000);

 /*Second Firing - bit 3*/
 mask=0x08;
 mirror=mirror|mask;
 *(unsigned char *) 0x7000=mirror;

 wait(1000);
 
 /*Braking - bit 4*/
 mask=0x10;
 mirror=mirror|mask;
 *(unsigned char *) 0x7000=mirror;

 wait(3000);
 
 /*Turn off all bits*/
 mask=0x00;
 mirror=mirror&mask;
 *(unsigned char *) 0x7000=mirror;
 /*************End of Accel&Brake********************/

 /*Reload rockets*/
 /*Accelerate and turn*/
 /*******************************************/
 while(BUMPER<120);
 wait(10000);
 
 /*Acceleration - bit 5*/
 mask=0x20;
 mirror=mirror|mask;
 *(unsigned char *) 0x7000=mirror;
 
 wait(1000);
 
 /*Turn - bit 7*/
 mask=0x80;
 mirror=mirror|mask;
 *(unsigned char *) 0x7000=mirror;
 
 wait(3000);
 
 /*Turn off all bits*/
 mask=0x00;
 mirror=mirror&mask;
 *(unsigned char *) 0x7000=mirror;
 /*************End of Accel&Turn********************/
   
}
/******************* End of Main ***********************/