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

Arm.c

ME 218B Project Hats Off

Team Firefox

March 7, 2010

Upper arm stepper = PS2, PS3, PP0, PP2

Lower arm servo = Timer0 PT2, Timer 6

DDRT = 0x4; //00000100 for servo

DDRP = 0x05; //00000101 for stepper 1=output

DDRS = 0x0C; //00001100 for stepper 1=output

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

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

#include "Arm.h"

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

#define CMD fullCmds

typedef enum { HIGH_ARM,

LOW_ARM

} ARM_t;

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

static void pulseServo(unsigned int width);

static void stepMotor(int);

static void StepMotorModule_runMotor(int, int);

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

static unsigned int servo_desPos; //lower arm servo

static unsigned int servo_curPos;

static unsigned int stepper_desPos; //upper arm stepper

static unsigned int stepper_curPos;

static unsigned int servo_lastCmdTime;

static unsigned int lastStepTime, curTime;

static int curStepStage;

static unsigned int skipSize;

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

const static ARM_t TargetHeights[16] = { HIGH_ARM, //0

HIGH_ARM, //1

LOW_ARM, //2

HIGH_ARM, //3

HIGH_ARM, //4

LOW_ARM, //5

HIGH_ARM, //6

HIGH_ARM, //7

HIGH_ARM, //8

HIGH_ARM, //9

LOW_ARM, //10

HIGH_ARM, //11

HIGH_ARM, //12

LOW_ARM, //13

HIGH_ARM, //14

HIGH_ARM};//15

//servo minPulseWidth = 1550 --> 0degrees

//servo maxPulseWidth = 8000 --> 180degrees

const static unsigned int cmdDelayMS = 22;

const static unsigned int servo_outPos = 1520; //7600; flip to swing the other way. also switch inequality sign in

//isArmStopped

const static unsigned int servo_homePos = 7600; //1520;

const static unsigned int servo_midPos = 4000;

const static unsigned int servo_waveOutPos = 3000;

const static unsigned int servo_waveInPos = 4000;

const static unsigned int stepper_highPos = 450;

const static unsigned int stepper_lowPos = 500;

const static unsigned int stepper_homePos = 0;

const static unsigned int stepper_topPos = 350;

const static unsigned int stepper_midPos = 100;

const static unsigned int stepper_attackPos = 220;

const static unsigned int servo_slowSkip = 120;

const static unsigned int servo_fastSkip = 200;

const static unsigned int servo_waveSkip = 80;

const static int stepper_waitTimeFast = 4;

const static int stepper_waitTimeSlow = 7;

static int CW = 1;

static int CCW = 0;

static int maxSteps = 4;

const static int fullCmds[] = {10, 9, 5, 6}; //full step drive

const static int waveCmds[] = {8, 1, 4, 2}; //Wave drive

const static int halfCmds[] = {10, 8, 9, 1, 5, 4, 6, 2}; //Half step drive

//Initializes OC on the servo to create a higher resolution PWM

void Arm_Init(void) {

MyTimer_Init(); //Already initializes Timer 0 to 3MHz

//TIM0_TSCR1 |= _S12_TEN; //Enable timer

//TIM0_TSCR2 = _S12_PR1 | _S12_PR0; //24MHz/8 = 3MHz

TIM0_TIOS |= _S12_IOS6; //Set timer0 channel 6 to OC

TIM0_TCTL1 |= _S12_OL6 | _S12_OM6; //Program a rise on compare

TIM0_TTOV |= _S12_TOV6; //Toggle bit6 on overflow

pulseServo(servo_homePos);

servo_desPos = servo_homePos; //Set servo to home position

stepper_desPos = stepper_homePos; //Set stepper to home position

lastStepTime = MyTimer_GetTime();

servo_lastCmdTime = MyTimer_GetTime();

curStepStage = 0;

PTS &= (BIT2LO); //Turn off both coils in stepper motor

PTS &= (BIT3LO);

PTP &= (BIT2LO);

PTP &= (BIT0LO);

}

//Steps the motor based on waitTime

static void StepMotorModule_runMotor(int waitTime, int dir) {

curTime = MyTimer_GetTime();

if ((curTime - lastStepTime) >= waitTime) {

stepMotor(dir);

lastStepTime = curTime;

}

}

//Steps the stepper motor using full step drive

static void stepMotor(int dir) {

char temp;

if (dir == 0){

curStepStage = curStepStage + 1; //Update current step in array

stepper_curPos++;

}

else{

curStepStage = curStepStage - 1; //Update current step in array

stepper_curPos--;

}

if (curStepStage < 0) curStepStage = maxSteps-1; //If overflow the array, loop back

if (curStepStage == maxSteps) curStepStage = 0;

temp = (char)(CMD[curStepStage]); //CMD = fullCmds array

//write out to 0 pin PS2

if (temp & BIT0HI) PTS |= BIT2HI;

else PTS &= BIT2LO;

//write out to 1 pin PS3

if (temp & BIT1HI) PTS |= BIT3HI;

else PTS &= BIT3LO;

//write out to 2 pin PP2

if (temp & BIT2HI) PTP |= BIT2HI;

else PTP &= BIT2LO;

//write out to 3 pin PP0

if (temp & BIT3HI) PTP |= BIT0HI;

else PTP &= BIT0LO;

}

//Set the pulse width of the rise time on OC.

//A change in width causes a change in servo angle

static void pulseServo(unsigned int width){

TIM0_TC6 = 0xffff-width; //Sets the pulse width

servo_curPos = width;

}

//Set servo and stepper to home position

void goToHomePosition(void){

stepper_desPos = stepper_homePos;

servo_desPos = servo_homePos;

}

//Set servo and stepper to attack position

void goToAttackPosition(void){

stepper_desPos = stepper_attackPos;

servo_desPos = servo_homePos;

}

//Begin the wave sequence in the servo and stepper

void waveArm(void){

stepper_desPos = stepper_attackPos;

servo_desPos = servo_outPos;

skipSize = servo_waveSkip;

}

//Checks if the arm has stopped waving.

//Returns 1 if desired position == current position for both stepper and servo

char isWaveStopped(void){

if (stepper_desPos != stepper_curPos){

if (stepper_desPos > stepper_curPos){ //Send arm out

StepMotorModule_runMotor(stepper_waitTimeFast, CCW);

}

else if (stepper_desPos < stepper_curPos){ //Bring arm in

if (stepper_curPos > stepper_topPos){ //When raising arm after swinging, step slower.

StepMotorModule_runMotor(stepper_waitTimeSlow, CW); //(otherwise it skips b/c it can't raise the arm that fast)

}

else //Step faster after passing the high point.

StepMotorModule_runMotor(stepper_waitTimeFast, CW);

}

}

if (servo_desPos != servo_curPos){

if ((MyTimer_GetTime() - servo_lastCmdTime) > cmdDelayMS){

servo_lastCmdTime = MyTimer_GetTime();

if (servo_desPos < servo_curPos){ //Wave out

if (servo_curPos <= servo_waveInPos) skipSize = servo_waveSkip; //Wave beginning point. Once past, move at

waveSkip

else skipSize = servo_slowSkip; //When not in wave region, move slow

pulseServo(servo_curPos - skipSize);

}

else if (servo_desPos > servo_curPos){ //Wave in

skipSize = servo_waveSkip;

pulseServo(servo_curPos + skipSize);

}

}

}

if (servo_curPos <= servo_waveOutPos){ //When servo gets to the end of the wave, bring servo back to wave beginning

servo_desPos = servo_waveInPos;

}

return ((servo_curPos == servo_desPos) && (stepper_curPos == stepper_desPos));

}

//Swing the arm to the target height (either high or low)

void swingArm(int targetNum) {

ARM_t pos;

pos = TargetHeights[targetNum];

if (pos == HIGH_ARM){

stepper_desPos = stepper_highPos;

skipSize = servo_slowSkip;

}

else{

stepper_desPos = stepper_lowPos;

skipSize = servo_slowSkip;

}

}

//Checks to see if the arm has stopped moving

char isArmStopped(void) {

if (stepper_desPos != stepper_curPos){

if (stepper_desPos > stepper_curPos){ //Need to raise arm

StepMotorModule_runMotor(stepper_waitTimeFast, CCW);

if (stepper_curPos >= stepper_attackPos+1) servo_desPos = servo_midPos; //Swing out the servo partway

if (stepper_curPos >= stepper_highPos) servo_desPos = servo_outPos; //Swing servo for attack

}

else if (stepper_desPos < stepper_curPos){ //Need to bring back in

if (stepper_curPos > stepper_topPos){

StepMotorModule_runMotor(stepper_waitTimeSlow, CW);

}

else

StepMotorModule_runMotor(stepper_waitTimeFast, CW);

}

}

if (servo_desPos != servo_curPos){ //Need to swing lower servo

if ((MyTimer_GetTime() - servo_lastCmdTime) > cmdDelayMS){

servo_lastCmdTime = MyTimer_GetTime();

if (servo_desPos > servo_curPos){ //Bring servo back

pulseServo(servo_curPos + skipSize);

if (servo_curPos >= servo_midPos) stepper_desPos = stepper_attackPos;

}

else if (servo_desPos < servo_curPos){ //Swing servo out

pulseServo(servo_curPos - skipSize);

}

}

}

if (servo_curPos <= servo_outPos){ //homePos = 7600, outPos = 1520

servo_desPos = servo_homePos;

}

return ((servo_curPos == servo_desPos) && (stepper_curPos == stepper_desPos));

}