/* System1Action.c
**
** Provides a example implementation of code to implement
** actions for a Visual State system.
*/
#include "System1Action.h" // Required action functions for System1
#include "Actions.h" // Export consistency check

#define printf(X)

#include "leftMotorPWM.h"
#include "rightMotorPWM.h"
#include "leftMotor.h"
#include "rightMotor.h"
//#include "Ports.h"

typedef unsigned char U8;
#define PTB (*(volatile U8 *) 0x0001)

// Port for LED Current State Display

#define STATUS (PTB) // Output port B
#define DDRSTATUS (DDRB) // Data direct

void initActions(void) {

// Configuring Port B for LEDs to Display Current State

DDRSTATUS = 0xFF ; // Force to output

}


VS_VOID eraserToggle (VS_INT pos){
VS_UINT16 upval = 1550;
VS_UINT16 downval = 600;
servoMotor_Enable( );
if (pos) {
servoMotor_SetDutyUS(downval);
}
else{
servoMotor_SetDutyUS(upval);
}
}

VS_VOID motorsForward (void){

//left motor forward
leftMotorPWM_Enable( );
leftMotorPWM_SetRatio16(0x6666); //60% duty cycle
leftMotor_PutVal(0x01);

//right motor forward
rightMotorPWM_Enable( );
rightMotorPWM_SetRatio16(0x6666); //60% duty cycle
rightMotor_PutVal(0x01);

}

VS_VOID motorsReverse (void){

//left motor back
leftMotorPWM_Enable( );
leftMotorPWM_SetRatio16(0x6666); //60% duty cycle
leftMotor_PutVal(0x02);

//right motor back
rightMotorPWM_Enable( );
rightMotorPWM_SetRatio16(0x6666); //60% duty cycle
rightMotor_PutVal(0x02);
}

VS_VOID motorsStop (void){

//left motor off
leftMotorPWM_Disable();
leftMotorPWM_SetRatio16(0);
leftMotor_PutVal(0x00);

//right motor off
rightMotorPWM_Disable();
rightMotorPWM_SetRatio16(0);
rightMotor_PutVal(0x00);

}

VS_VOID rotateLeft (void){

//left motor back
leftMotorPWM_Enable( );
leftMotorPWM_SetRatio16(0x6666); //60% duty cycle
leftMotor_PutVal(0x02);

//right motor forward
rightMotorPWM_Enable( );
rightMotorPWM_SetRatio16(0x6666); //60% duty cycle
rightMotor_PutVal(0x01);

}

VS_VOID rotateRight (void){

//left motor forward
leftMotorPWM_Enable( );
leftMotorPWM_SetRatio16(0x6666); //60% duty cycle
leftMotor_PutVal(0x01);

//right motor back
rightMotorPWM_Enable( );
rightMotorPWM_SetRatio16(0x6666); //60% duty cycle
rightMotor_PutVal(0x02);

}

VS_VOID turnLeftBack (void){

//left motor back - slow
leftMotorPWM_Enable( );
leftMotorPWM_SetRatio16(0x9999); //duty cycle changed to 40%
leftMotor_PutVal(0x02);

//right motor back - fast
rightMotorPWM_Enable( );
rightMotorPWM_SetRatio16(0x6666); //60% duty cycle
rightMotor_PutVal(0x02);

}

VS_VOID turnLeftForward (void){

//left motor forward - slow
leftMotorPWM_Enable( );
leftMotorPWM_SetRatio16(0x9999); //duty cycle changed to 40%
leftMotor_PutVal(0x01);

//right motor forward - fast
rightMotorPWM_Enable( );
rightMotorPWM_SetRatio16(0x6666); //60% duty cycle
rightMotor_PutVal(0x01);
}

VS_VOID turnRightBack (void){

//left motor back - fast
leftMotorPWM_Enable( );
leftMotorPWM_SetRatio16(0x6666); //60% duty cycle
leftMotor_PutVal(0x02);

//right motor back - slow
rightMotorPWM_Enable( );
rightMotorPWM_SetRatio16(0x9999); //duty cycle changed to 40%
rightMotor_PutVal(0x02);
}

VS_VOID turnRightForward (void){

//left motor forward - fast
leftMotorPWM_Enable( );
leftMotorPWM_SetRatio16(0x6666); //60% duty cycle
leftMotor_PutVal(0x01);

//right motor forward - slow
rightMotorPWM_Enable( );
rightMotorPWM_SetRatio16(0x9999); //duty cycle changed to 40%
rightMotor_PutVal(0x01);

}

VS_VOID displayState (VS_INT currentState) {
STATUS = currentState;
}


/* EventGenerator.c
**
** Provides a simplistic implementation of code to monitor inputs
** and generate events for a Visual State system.
**
** Should really use beans interface for Metrowerks!
**
*/

#include "SimpleEventQueue.h" // Event Q Access
#include "System1Data.h" // Event names
#include "InputEventGenerator.h" // Export consistency check
#include "Ports.h"

// Port for Bump Sensors

#define INPUT (PTH) // Input port H
#define DDRINPUT (DDRH) // Data direct
#define PERINPUT (PERH) // Pull-device enable
#define PPSINPUT (PPSH) // Pull up/down select

// Port for Light Sensors

#define INPUT2 (PTP) // Input port P
#define DDRINPUT2 (DDRP) // Data direct
#define PERINPUT2 (PERP) // Pull-device enable
#define PPSINPUT2 (PPSP) // Pull up/down select

//Bump Sensors' Masks

#define B (0x10) // pin 4 Back
#define F (0x40) // pin 6 Front
#define LF (0x80) // pin 7 Left Front
#define LB (0x08) // pin 3 Left Back
#define RF (0x04) // pin 2 Right Front
#define RB (0x20) // pin 5 Right Back)
#define RB (0x11) // Right Back
//Light Sensors' Masks

#define EL (0x01) // pin 0 Extreme Left
#define L (0x02) // pin 1 Left
#define C (0x04) // pin 2 Center
#define R (0x08) // pin 3 Right
#define ER (0x10) // pin 4 Extreme Right

void initInputEvents(void) {

// 6 Bump Sensors as inputs.

DDRINPUT &= ~(B&F&LF&LB&RF&RB); // Force to inputs
PERINPUT |= (B&F&LF&LB&RF&RB); // Pull-device enable
PPSINPUT &= ~(B&F&LF&LB&RF&RB); // Pull direction = UP

// 5 Light Sensors as inputs

DDRINPUT2 &= ~(EL&L&C&R&ER); // Force to inputs
PERINPUT2 |= (EL&L&C&R&ER); // Pull-device enable
PPSINPUT2 &= ~(EL&L&C&R&ER); // Pull direction = UP
}

void checkInputs(void) {
/*
** Called every 10 mS from timer code
**
** Checks inputs for changes and creates events for the state machine input queue.
**
*/


static U8 firstSample = 0x00; // input values from 2 interrupts ago
static U8 secondSample = 0x00; // input values on last interrupt
U8 thirdSample = ~INPUT; // input values on this interrupt
//bump sensors active low

U8 stable = ~(thirdSample ^ secondSample); // Indicates currently stable inputs
U8 delta = secondSample ^ firstSample; // Indicates previously changed inputs

U8 stableDelta = stable & delta; // Indicates bits that have changed and are now stable
// ie need to generate events for


// Generate events for each input that has changed to 1 and is now stable.
// (Could equally well generate events on 0 as well i.e. both press & release)

// Generating events for input from bump sensors

if (stableDelta & B)
if (thirdSample & B) {
SEQ_AddEvent(evB);
}

if (stableDelta & F)
if (thirdSample & F) {
SEQ_AddEvent(evF);
}

if (stableDelta & LF)
if (thirdSample & LF) {
SEQ_AddEvent(evLF);
}

if (stableDelta & LB)
if (thirdSample & LB) {
SEQ_AddEvent(evLB);
}

if (stableDelta & RF)
if (thirdSample & RF) {
SEQ_AddEvent(evRF);
}

if (stableDelta & RB)
if (thirdSample & RB) {
SEQ_AddEvent(evRB);
}

//Generating events for input from light sensors

if (INPUT2 & EL)
{
SEQ_AddEvent(evLFL);
}

if (INPUT2 & L)
{
SEQ_AddEvent(evLL);
}

if (INPUT2 & C)
{
SEQ_AddEvent(evCL);
}

if (INPUT2 & R)
{
SEQ_AddEvent(evRL);
}

if (INPUT2 & ER)
{
SEQ_AddEvent(evRLR);
}

firstSample = secondSample;
secondSample = thirdSample;

}