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

StepperMotor.c

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

#include "StepperMotor.h"

/*-------------------------- Module Constants -----------------------------*/

const static char hardGrapple = 1; //Boolean used to invert the grapple control dial to make

//the game more difficult

const static unsigned int dialLO = 15; //Zero position on raw analog input from dial

const static unsigned int dialHI = 1005; //Max position on raw analog input from dial

const static unsigned int minWait = 2; //max motor speed

const static unsigned int maxWait = 10; //min motor speed

const static unsigned int resetWait = 5; //reset motor speed

const static unsigned int deadZone_pct10 = 30; //Portion of analog dial that reads neutral (commands

//no motor speed); this must be greater than 25

const static unsigned int victorySpeed = 15; //victory motor speed

const static int startPosOffset = 23; //Start position for Tom Cruise

const static int topRangePos = 593; //Highest position that Tom can begin download

const static int bottomRangePos = 643; //Lowest position that Tom can begin download

const static int hitFloorPos = 667; //Hit floor position

/*------------------------- Module Definitions ----------------------------*/

#define LOWER_DIR 0 //Dir command to stepper motor that results in motor rotation that lowers Tom

#define RAISE_DIR 1 //Dir command to stepper motor that results in motor rotation that raises Tom

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

static int lastCalPt; //used only for test function

static int motorPos; //Motor position (always positive with 0 being top)

static char motorDir; //Indicates which direction the motor is current set at

static int motorSpd; //Indicates current speed of the motor

static char waitingForDialNeutral; //Indicates if dial started in non neutral position

static unsigned int lastClockTime; //Timing variable for motor speed control

static unsigned int lastMotorPrint; //Timing variable for test function

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

static void pulseClock(void); //Pulses the clock line to the stepper motor driver

static int readDial(void); //Reads analog dial as a number between 0 and 1000

static void setDir(char); //Sets the direction line to the stepper motor driver

static void runMotor(int); //Runs the motor at the given speed (input int is actually the

//wait time, so speed = 1/(wait time) )

/*---------------------------- Module Code ------------------------------*/

void initStepMotor(int howManySteps) {

int dialPos;

lastClockTime = TMRS12_GetTime(); //Upate timing variables

lastMotorPrint = TMRS12_GetTime();

//Zero out position

motorSpd = 0;

motorPos = 0;

setDir(RAISE_DIR); //Set motor direction to raise Tom Cruise

while (motorPos > (-1)*(howManySteps)) { //Run motor into stop for a specified # of steps

runMotor(resetWait); //Run motor at a calibrated reset speed

}

motorPos = 0; //Reset zero position

//Move to calibrated start position

setDir(LOWER_DIR); //Set motor direction to lower Tom Cruise

while (motorPos != startPosOffset) { //Continue lowering him until he is at the start

runMotor(resetWait); //position

}

motorPos = 0; //Reset zero position

//Check if dial is not in neutral

dialPos = readDial();

if ((dialPos > (500 + deadZone_pct10 - 5)) || (dialPos < (500 - deadZone_pct10 + 5))) {

waitingForDialNeutral = 1; //Dial not in neutral position, make note to ignore dial input

//until it passes through neutral position

}

else {

waitingForDialNeutral = 0; //Dial is ready to go

}

//testing only

lastCalPt = -1;

}

MOTOR_STATUS_t resetStepMotorStatus(void) {

if (motorPos <= 0) {

return MOTOR_NOT_IN_RANGE_STATUS;

}

else {

setDir(RAISE_DIR);

runMotor(resetWait);

return MOTOR_RESETTING_STATUS;

}

}

MOTOR_STATUS_t slowVictoryReset(void) {

if (motorPos <= 0) {

return MOTOR_NOT_IN_RANGE_STATUS ;

}

else {

setDir(RAISE_DIR);

runMotor(victorySpeed);

return MOTOR_RESETTING_STATUS;

}

}

MOTOR_STATUS_t checkStepMotorStatus(void) {

int dialPos = 0;

int relativeDialPos = 0;

int cmdWaitTime = 0;

unsigned long myTemp = 0; //Use to avoid floating-point math

//Read in analog dial

dialPos = readDial(); //Will return a number between 0 and 1000

// Determine commanded direction and speed

if (dialPos < (500 - deadZone_pct10)){ //Check if dial is above the neutral (aka dead) zone

//Raise grapple

setDir(LOWER_DIR);

if (hardGrapple) //Interpret dial commands “backwards” if hard grapple is activated

relativeDialPos = dialPos + 1;

else

relativeDialPos = (500 - deadZone_pct10) - dialPos;

}

else {

if (dialPos > (500 + deadZone_pct10)){ //Check if dial is below the neutral (aka dead) zone

//Lower grapple

setDir(RAISE_DIR);

if (hardGrapple) //Interpret dial commands “backwards” if hard grapple is activated

relativeDialPos = 1001 - dialPos;

else

relativeDialPos = dialPos - (500 + deadZone_pct10);

}

else {

relativeDialPos = 0; //If dial is in neutral (aka dead) zone, don’t command any speed

}

}

//Need to get a neutral reading before allowing dial to control grapple (this prevents the

//grapple from running before the user has ever touched the dial)

if (waitingForDialNeutral) {

//Check if dial is in neutral (aka dead) zone plus some hysteresis

if ((dialPos > (500 + deadZone_pct10 - 25)) || (dialPos < (500 - deadZone_pct10 + 25)))

relativeDialPos = 0; //cancel dial command until neutral is found

else

waitingForDialNeutral = 0;

}

//Calculate speed (which is actually commanded by wait time); commanded wait time will be

//something between the calibrated max and min wait times (corresponding to the min and max

//speeds of the motor)

myTemp = ((long)maxWait-minWait)*(relativeDialPos);

cmdWaitTime = maxWait - (int)(myTemp/(500 - deadZone_pct10));

//Before commanding motor, check limits; don’t allow motor to go up passed zero position, or

//lower than the floor

if (((motorDir == RAISE_DIR) && (motorPos > 0)) ||

((motorDir == LOWER_DIR) && (motorPos < hitFloorPos))) {

if (relativeDialPos != 0) {

runMotor(cmdWaitTime); //Command motor to be run at specified speed = 1/cmdWaitTime

}

}

//Check current motor position to determine if an event has occured

if (motorPos >= hitFloorPos) {

return MOTOR_HIT_FLOOR_STATUS;

}

else {

if ((motorPos >= topRangePos) && (motorPos <= bottomRangePos))

return MOTOR_IN_RANGE_STATUS;

}

else {

return MOTOR_NOT_IN_RANGE_STATUS;

}

}

}

static void runMotor(int cmdWaitTime) {

unsigned int myTime = 0;

int filteredWaitTime = 0;

//Filter input speed to prevent instananous accelerations of the stepper motor

filteredWaitTime = motorSpd;

if (cmdWaitTime < motorSpd)

filteredWaitTime = motorSpd - 1;

if (cmdWaitTime > motorSpd)

filteredWaitTime = motorSpd + 1;

myTime = TMRS12_GetTime();

if ((myTime-lastClockTime) >= filteredWaitTime) { //If enough time has passed, command a step

pulseClock();

motorSpd = filteredWaitTime; //Save current speed for future filtering

lastClockTime = myTime; //Update timing variables

if (motorDir == LOWER_DIR) //Update motor position

motorPos++;

else

motorPos--;

}

}

static int readDial(void) {

int myPos = 0;

unsigned long myTemp = 0; //Used to avoid floating point math

int myCalc = 0;

myPos = ADS12_ReadADPin(0); //Read in analog dial as number between 0 and 1023

if (myPos <= dialLO) //Check against calibrated zero position on dial

return 0;

else {

if (myPos >= dialHI) //Check against calibrated fully on position on dial

return 1000;

else { //Linearize between the two extreme calibrations

myTemp = (1000*((long)myPos-dialLO));

myCalc = (int)((myTemp)/((dialHI-dialLO)));

return myCalc;

}

}

}

static void setDir(char dir) {

motorDir = dir;

if (dir)

PTAD = PTAD | BIT2HI; //Set stepper motor driver direction line to HI

else

PTAD = PTAD & BIT2LO; //Set stepper motor driver direction line to LO

}

static void pulseClock(void) {

int j = 0;

PTAD = PTAD | BIT1HI; //Set step command line to HI

//delay slightly to ensure CLOCK output stays HI for > 10[us]

for (j=1; j<200; j++) {

}

PTAD = PTAD & BIT1LO; //Set step command line to LO

}

//test function

void calibrateDial(void) {

int myPos = 0;

myPos = readDial();

if (myPos != lastCalPt) {

lastCalPt = myPos;

printf("Dial: %d\r\n", myPos);

}

}

//test function

void showMotorPos(void) {

unsigned int curTime;

curTime = TMRS12_GetTime();

if (curTime > (lastMotorPrint + 500)){

lastMotorPrint = curTime;

printf("MotorPos: %d\r\n", motorPos);

}

}