ShootPowerSM.c
//****************************************************************************
Module
ShootTargetSM.c
Revision
2.0.1
Description
This is a template file for implementing state machines.
Notes
History
When Who What/Why
-------------- --- --------
02/13/13 02:32 mt ME218B Project
02/08/12 09:56 jec revisions for the Events and Services Framework Gen2
02/13/10 14:29 jec revised Start and run to add new kind of entry function
to make implementing history entry cleaner
02/13/10 12:29 jec added NewEvent local variable to During function and
comments about using either it or Event as the return
02/11/10 15:54 jec more revised comments, removing last comment in during
function that belongs in the run function
02/09/10 17:21 jec updated comments about internal transitions on During funtion
02/18/09 10:14 jec removed redundant call to RunLowerlevelSM in EV_Entry
processing in During function
02/20/07 21:37 jec converted to use enumerated type for events & states
02/13/05 19:38 jec added support for self-transitions, reworked
to eliminate repeated transition code
02/11/05 16:54 jec converted to implment hierarchy explicitly
02/25/03 10:32 jec converted to take a passed event parameter
02/18/99 10:19 jec built template from MasterMachine.c
02/14/99 10:34 jec Began Coding
****************************************************************************/
/*----------------------------- Include Files -----------------------------*/
// Basic includes for a program using the Events and Services Framework
#include "ES_Configure.h"
#include "ES_Framework.h"
#include "ES_ServiceHeaders.h"
/* include header files for this state machine as well as any machines at the
next lower level in the hierarchy that are sub-machines to this machine
*/
/*----------------------------- Module Defines ----------------------------*/
// define constants for the states for this machine
// and any other local defines
#define DistScale 20
/*---------------------------- Module Functions ---------------------------*/
/* prototypes for private functions for this machine, things like during
functions, entry & exit functions.They should be functions relevant to the
behavior of this state machine
*/
static ES_Event DuringSpinForAlign( ES_Event Event);
static ES_Event DuringShooting( ES_Event Event);
static ES_Event DuringWaitForServo( ES_Event Event);
static ES_Event DuringWaitForLoader( ES_Event Event);
/*---------------------------- Module Variables ---------------------------*/
// everybody needs a state variable, you may need others as well
static PowerState_t CurrentState = EndState;
static coord TargetCoord =
{
0, 0, 0, 0
};
static coord OurBot;
static int angleForPower = 0;
static int servo;
static coord PowerStation;
static int SweepCount = 0;
/*------------------------------ Module Code ------------------------------*/
/****************************************************************************
Function
RunShootPowerSM
Parameters
ES_Event: the event to process
Returns
ES_Event: an event to return
Description
add your description here
Notes
uses nested switch/case to implement the machine.
Author
Michelle T, 2/13/13
****************************************************************************/
ES_Event RunShootPowerSM( ES_Event CurrentEvent )
{
unsigned char MakeTransition = False;/* are we making a state transition? */
ShootingState_t NextState = CurrentState;
ES_Event EntryEventKind = { ES_ENTRY, 0 };// default to normal entry to new state
ES_Event ReturnEvent = CurrentEvent; // assume we are not consuming event
ES_Event currEvent;
int angle;
if (CurrentEvent.EventType == GameEnd)
{
NextState = EndState;
MakeTransition = True;
EntryEventKind.EventType = ES_ENTRY;
ReturnEvent = CurrentEvent;
}
switch ( CurrentState )
{
case SpinForAlign:
CurrentEvent = DuringSpinForAlign(CurrentEvent);
if(CurrentEvent.EventType == Aligned)
{
Stop();
NextState = WaitForLoader;
MakeTransition = True;
EntryEventKind.EventType = ES_ENTRY;
ReturnEvent = CurrentEvent;
break;
}
break;
case WaitForServo:
CurrentEvent = DuringWaitForServo(CurrentEvent);
if(CurrentEvent.EventType == ES_TIMEOUT && CurrentEvent.EventParam == 5)
{
NextState = WaitForLoader;
MakeTransition = True;
EntryEventKind.EventType = ES_ENTRY;
ReturnEvent = CurrentEvent;
break;
}
break;
case WaitForLoader:
CurrentEvent = DuringWaitForLoader(CurrentEvent);
if(CurrentEvent.EventType == ES_TIMEOUT && CurrentEvent.EventParam == 2)
{
NextState = Shooting;
MakeTransition = True;
EntryEventKind.EventType = ES_ENTRY;
ReturnEvent = CurrentEvent;
break;
}
break;
case Shooting:
CurrentEvent = DuringShooting(CurrentEvent);
if(CurrentEvent.EventType == ES_TIMEOUT && CurrentEvent.EventParam == 2)
{
printf("resetLoad\r\n");
resetLoad();
//CloakBot();
printf("changeAmmo\r\n");
changeAmmo(-1);
currEvent.EventType = BallFired;
PostMasterSM(currEvent);
NextState = EndState;
MakeTransition = True;
EntryEventKind.EventType = ES_ENTRY;
ReturnEvent = CurrentEvent;
break;
}
break;
case EndState:
break;
// repeat state pattern as required for other states
}
// If we are making a state transition
if (MakeTransition == True)
{
// Execute exit function for current state
CurrentEvent.EventType = ES_EXIT;
RunShootPowerSM(CurrentEvent);
CurrentState = NextState; //Modify state variable
// Execute entry function for new state
// this defaults to ES_ENTRY
RunShootPowerSM(EntryEventKind);
}
return(CurrentEvent);
}
/****************************************************************************
Function
StartShootPowerSM
Parameters
None
Returns
None
Description
Does any required initialization for this state machine
Notes
Author
J. Edward Carryer, 2/18/99, 10:38AM
****************************************************************************/
void StartShootPowerSM ( ES_Event CurrentEvent )
{
// local variable to get debugger to display the value of CurrentEvent
ES_Event LocalEvent = CurrentEvent;
// to implement entry to a history state or directly to a substate
// you can modify the initialization of the CurrentState variable
// otherwise just start in the entry state every time the state machine
// is started
if ( ES_ENTRY_HISTORY != CurrentEvent.EventType )
{
if (angleForPower == 0)
{
CurrentState = SpinForAlign;
} else
{
CurrentState = SpinForAlign;
// CurrentState = WaitForServo;
}
}
// call the entry function (if any) for the ENTRY_STATE
RunShootPowerSM(CurrentEvent);
}
/****************************************************************************
Function
QueryShootTargetSM
Parameters
None
Returns
TemplateState_t The current state of the Template state machine
Description
returns the current state of the Template state machine
Notes
Author
J. Edward Carryer, 2/11/05, 10:38AM
****************************************************************************/
ShootingState_t QueryShootPowerSM ( void )
{
return(CurrentState);
}
/***************************************************************************
private functions
***************************************************************************/
static ES_Event DuringSpinForAlign( ES_Event Event)
{
ES_Event ReturnEvent = Event; // assmes no re-mapping or comsumption
static int updown = 1;
static int flag = 0;
if (Event.EventType == ES_ENTRY )
{
//Moves servo into position to begin sweeping
printf("In SpinForAlign\r\n");
UncloakBot();
servo = 55;
Servo_SetAngle(AimServo, servo);
//Timer to wait for servo to be in position
ES_Timer_SetTimer(5, 800);
ES_Timer_StartTimer(5);
updateFAC();
//printf("Start Timer 5\r\n");
}
else if ( Event.EventType == ES_EXIT )
{
//Once found, set motor speed
OurBot = queryShip(15);
setShooterSpeed_PowerSM();//angleForPower = servo;
SweepCount = 0;
} else if( Event.EventType == ES_TIMEOUT && Event.EventParam == 1) // finished rotating
{
if (flag == 0) //Wait for 600 ms before updating the FAC, to make sure the coordinates are correct
{
ES_Timer_SetTimer(1,600);
ES_Timer_StartTimer(1);
flag = 1;
} else
{
updateFAC(); // added
printf("UpdatingFAC in ShootPower\r\n");
flag = 0;
}
}else if(Event.EventType == UpdateFinish) //after the wait and update, get our coordinates and request align from event checkers
{
printf("Updating OurBot in ShootPower\r\n");
OurBot = queryShip(15);
if (SweepCount != 0) requestAlign();
}
else if( Event.EventType == ES_TIMEOUT && Event.EventParam == 5)
{
if (servo == 55) requestAlign();
//Sweep servo up and down
if (updown == 1)
{
Servo_SetAngle(AimServo, ++servo);
if (servo >= 125) updown = 0;
} else
{
Servo_SetAngle(AimServo, --servo);
if (servo <= 55)
{
updown = 1;
SweepCount++; // to keep track of when to rotate actual bot if we can't find the beacon in the current sector
if (SweepCount == 2)
{
RotateCW(45, 1);
TIM0_TIE &= ~_S12_C4I;
}
if (SweepCount == 4)
{
RotateCCW(90, 1);
TIM0_TIE &= ~_S12_C4I;
}
if (SweepCount == 6)
{
SweepCount = 0;
}
}
}
printf("Servo angle = %d\r", servo);
ES_Timer_SetTimer(5, 20);
ES_Timer_StartTimer(5);
// printf("Start Timer 5\r\n");
} else if (Event.EventType == Aligned)
{
//Adjustment for mechanical difference
if(updown == 1)
{
servo += 5;
Servo_SetAngle(AimServo, servo);
} else
{
servo += 5;
Servo_SetAngle(AimServo, servo);
}
} else
// do the 'during' function for this state
{
}
// return either Event, if you don't want to allow the lower level machine
// to remap the current event, or ReturnEvent if you do want to allow it.
return(ReturnEvent);
}
static ES_Event DuringWaitForServo( ES_Event Event)
{
ES_Event ReturnEvent = Event; // assmes no re-mapping or comsumption
if (Event.EventType == ES_ENTRY )
{
printf("In WaitForServo\r\n");
UncloakBot(); //Never implemented
Servo_SetAngle(AimServo, angleForPower);
ES_Timer_SetTimer(5, 1000);
ES_Timer_StartTimer(5);
}
else if ( Event.EventType == ES_EXIT )
{
}else
{
}
return(ReturnEvent);
}
static ES_Event DuringWaitForLoader( ES_Event Event)
{
ES_Event ReturnEvent = Event; // assmes no re-mapping or comsumption
if (Event.EventType == ES_ENTRY )
{
printf("In WaitForLoader\r\n");
printf("Load shooter\r\n");
loadShooter();
ES_Timer_SetTimer(2, 3000);//Timer especially long for increased PWM, used mainly for grading session
ES_Timer_StartTimer(2);
printf("Start Timer 2\r\n");
}
else if ( Event.EventType == ES_EXIT )
{
}else
{
}
return(ReturnEvent);
}
static ES_Event DuringShooting( ES_Event Event)
{
ES_Event ReturnEvent = Event; // assmes no re-mapping or comsumption
ES_Event currEvent;
if (Event.EventType == ES_ENTRY)
{
printf("In Shooting\r\n");
printf("Shoot ball\r\n");
shootBall();
ES_Timer_SetTimer(2,300);
ES_Timer_StartTimer(2);
printf("Start Timer 2\r\n");
}
else if ( Event.EventType == ES_EXIT )
{
} else
{
}
return(ReturnEvent);
}
void setShooterSpeed_PowerSM( void)
{
double Current_BatVoltage;
double ShooterMotorPWM;
double PowerStationDistance;
int ShooterAngle;
// static int FirstTime = 0;
int botangle;
if(getColor() ==2){ //blue
PowerStation.x = 0;
} else if (getColor()==1) { //red
PowerStation.x = FAC_MAX_X;
}
if (getColor() == 1) //red
{
botangle = 270-OurBot.r*45/32;
ShooterAngle = (servo-90);
PowerStation.y = OurBot.y - (OurBot.x - PowerStation.x)*tan((ShooterAngle + botangle)*3.14/180); // *tan(servo - ourbot.r)
} else // blue
{
botangle = 90- OurBot.r*45/32;//= 45/32*OurBot.r - 90;
ShooterAngle =(servo-90);
PowerStation.y = OurBot.y - (OurBot.x - PowerStation.x)*tan((ShooterAngle + botangle)*3.14/180); // *tan(servo - ourbot.r)
if(PowerStation.y > 255) PowerStation.y =255;
else if (PowerStation.y <0) PowerStation.y = 0;
}
printf("Powery %d ShooterAng %d botangle %d \r\n",PowerStation.y, (int)ShooterAngle, (int)botangle);
printf("Ourbot x %d y %d r %d\r\n", OurBot.x, OurBot.y, OurBot.r);
// PowerStation.y = OurBot.y - (OurBot.x - PowerStation.x)*tan(ShooterAngle + botangle); // *tan(servo - ourbot.r)
PowerStationDistance = (double)sqrt((pow((OurBot.y-PowerStation.y),2) + pow((OurBot.x-PowerStation.x),2))); //Max should be about 190
Current_BatVoltage = (double)ADS12_ReadADPin(4);
ShooterMotorPWM = (CAL_BAT_VOLTAGE/Current_BatVoltage)*(PowerStationDistance*SHOOTER_DISTANCE_SCALE_M + SHOOTER_DISTANCE_SCALE_B);
SetDuty((int)ShooterMotorPWM,1);
printf("ShooterMotorPWM %d PowerStationdistance %d PowerStationy %d OurBoty %d\r\n", (int)ShooterMotorPWM, (int)PowerStationDistance, PowerStation.y, OurBot.y);
}
/*******************************
Public Functions
*******************************/
Module
ShootTargetSM.c
Revision
2.0.1
Description
This is a template file for implementing state machines.
Notes
History
When Who What/Why
-------------- --- --------
02/13/13 02:32 mt ME218B Project
02/08/12 09:56 jec revisions for the Events and Services Framework Gen2
02/13/10 14:29 jec revised Start and run to add new kind of entry function
to make implementing history entry cleaner
02/13/10 12:29 jec added NewEvent local variable to During function and
comments about using either it or Event as the return
02/11/10 15:54 jec more revised comments, removing last comment in during
function that belongs in the run function
02/09/10 17:21 jec updated comments about internal transitions on During funtion
02/18/09 10:14 jec removed redundant call to RunLowerlevelSM in EV_Entry
processing in During function
02/20/07 21:37 jec converted to use enumerated type for events & states
02/13/05 19:38 jec added support for self-transitions, reworked
to eliminate repeated transition code
02/11/05 16:54 jec converted to implment hierarchy explicitly
02/25/03 10:32 jec converted to take a passed event parameter
02/18/99 10:19 jec built template from MasterMachine.c
02/14/99 10:34 jec Began Coding
****************************************************************************/
/*----------------------------- Include Files -----------------------------*/
// Basic includes for a program using the Events and Services Framework
#include "ES_Configure.h"
#include "ES_Framework.h"
#include "ES_ServiceHeaders.h"
/* include header files for this state machine as well as any machines at the
next lower level in the hierarchy that are sub-machines to this machine
*/
/*----------------------------- Module Defines ----------------------------*/
// define constants for the states for this machine
// and any other local defines
#define DistScale 20
/*---------------------------- Module Functions ---------------------------*/
/* prototypes for private functions for this machine, things like during
functions, entry & exit functions.They should be functions relevant to the
behavior of this state machine
*/
static ES_Event DuringSpinForAlign( ES_Event Event);
static ES_Event DuringShooting( ES_Event Event);
static ES_Event DuringWaitForServo( ES_Event Event);
static ES_Event DuringWaitForLoader( ES_Event Event);
/*---------------------------- Module Variables ---------------------------*/
// everybody needs a state variable, you may need others as well
static PowerState_t CurrentState = EndState;
static coord TargetCoord =
{
0, 0, 0, 0
};
static coord OurBot;
static int angleForPower = 0;
static int servo;
static coord PowerStation;
static int SweepCount = 0;
/*------------------------------ Module Code ------------------------------*/
/****************************************************************************
Function
RunShootPowerSM
Parameters
ES_Event: the event to process
Returns
ES_Event: an event to return
Description
add your description here
Notes
uses nested switch/case to implement the machine.
Author
Michelle T, 2/13/13
****************************************************************************/
ES_Event RunShootPowerSM( ES_Event CurrentEvent )
{
unsigned char MakeTransition = False;/* are we making a state transition? */
ShootingState_t NextState = CurrentState;
ES_Event EntryEventKind = { ES_ENTRY, 0 };// default to normal entry to new state
ES_Event ReturnEvent = CurrentEvent; // assume we are not consuming event
ES_Event currEvent;
int angle;
if (CurrentEvent.EventType == GameEnd)
{
NextState = EndState;
MakeTransition = True;
EntryEventKind.EventType = ES_ENTRY;
ReturnEvent = CurrentEvent;
}
switch ( CurrentState )
{
case SpinForAlign:
CurrentEvent = DuringSpinForAlign(CurrentEvent);
if(CurrentEvent.EventType == Aligned)
{
Stop();
NextState = WaitForLoader;
MakeTransition = True;
EntryEventKind.EventType = ES_ENTRY;
ReturnEvent = CurrentEvent;
break;
}
break;
case WaitForServo:
CurrentEvent = DuringWaitForServo(CurrentEvent);
if(CurrentEvent.EventType == ES_TIMEOUT && CurrentEvent.EventParam == 5)
{
NextState = WaitForLoader;
MakeTransition = True;
EntryEventKind.EventType = ES_ENTRY;
ReturnEvent = CurrentEvent;
break;
}
break;
case WaitForLoader:
CurrentEvent = DuringWaitForLoader(CurrentEvent);
if(CurrentEvent.EventType == ES_TIMEOUT && CurrentEvent.EventParam == 2)
{
NextState = Shooting;
MakeTransition = True;
EntryEventKind.EventType = ES_ENTRY;
ReturnEvent = CurrentEvent;
break;
}
break;
case Shooting:
CurrentEvent = DuringShooting(CurrentEvent);
if(CurrentEvent.EventType == ES_TIMEOUT && CurrentEvent.EventParam == 2)
{
printf("resetLoad\r\n");
resetLoad();
//CloakBot();
printf("changeAmmo\r\n");
changeAmmo(-1);
currEvent.EventType = BallFired;
PostMasterSM(currEvent);
NextState = EndState;
MakeTransition = True;
EntryEventKind.EventType = ES_ENTRY;
ReturnEvent = CurrentEvent;
break;
}
break;
case EndState:
break;
// repeat state pattern as required for other states
}
// If we are making a state transition
if (MakeTransition == True)
{
// Execute exit function for current state
CurrentEvent.EventType = ES_EXIT;
RunShootPowerSM(CurrentEvent);
CurrentState = NextState; //Modify state variable
// Execute entry function for new state
// this defaults to ES_ENTRY
RunShootPowerSM(EntryEventKind);
}
return(CurrentEvent);
}
/****************************************************************************
Function
StartShootPowerSM
Parameters
None
Returns
None
Description
Does any required initialization for this state machine
Notes
Author
J. Edward Carryer, 2/18/99, 10:38AM
****************************************************************************/
void StartShootPowerSM ( ES_Event CurrentEvent )
{
// local variable to get debugger to display the value of CurrentEvent
ES_Event LocalEvent = CurrentEvent;
// to implement entry to a history state or directly to a substate
// you can modify the initialization of the CurrentState variable
// otherwise just start in the entry state every time the state machine
// is started
if ( ES_ENTRY_HISTORY != CurrentEvent.EventType )
{
if (angleForPower == 0)
{
CurrentState = SpinForAlign;
} else
{
CurrentState = SpinForAlign;
// CurrentState = WaitForServo;
}
}
// call the entry function (if any) for the ENTRY_STATE
RunShootPowerSM(CurrentEvent);
}
/****************************************************************************
Function
QueryShootTargetSM
Parameters
None
Returns
TemplateState_t The current state of the Template state machine
Description
returns the current state of the Template state machine
Notes
Author
J. Edward Carryer, 2/11/05, 10:38AM
****************************************************************************/
ShootingState_t QueryShootPowerSM ( void )
{
return(CurrentState);
}
/***************************************************************************
private functions
***************************************************************************/
static ES_Event DuringSpinForAlign( ES_Event Event)
{
ES_Event ReturnEvent = Event; // assmes no re-mapping or comsumption
static int updown = 1;
static int flag = 0;
if (Event.EventType == ES_ENTRY )
{
//Moves servo into position to begin sweeping
printf("In SpinForAlign\r\n");
UncloakBot();
servo = 55;
Servo_SetAngle(AimServo, servo);
//Timer to wait for servo to be in position
ES_Timer_SetTimer(5, 800);
ES_Timer_StartTimer(5);
updateFAC();
//printf("Start Timer 5\r\n");
}
else if ( Event.EventType == ES_EXIT )
{
//Once found, set motor speed
OurBot = queryShip(15);
setShooterSpeed_PowerSM();//angleForPower = servo;
SweepCount = 0;
} else if( Event.EventType == ES_TIMEOUT && Event.EventParam == 1) // finished rotating
{
if (flag == 0) //Wait for 600 ms before updating the FAC, to make sure the coordinates are correct
{
ES_Timer_SetTimer(1,600);
ES_Timer_StartTimer(1);
flag = 1;
} else
{
updateFAC(); // added
printf("UpdatingFAC in ShootPower\r\n");
flag = 0;
}
}else if(Event.EventType == UpdateFinish) //after the wait and update, get our coordinates and request align from event checkers
{
printf("Updating OurBot in ShootPower\r\n");
OurBot = queryShip(15);
if (SweepCount != 0) requestAlign();
}
else if( Event.EventType == ES_TIMEOUT && Event.EventParam == 5)
{
if (servo == 55) requestAlign();
//Sweep servo up and down
if (updown == 1)
{
Servo_SetAngle(AimServo, ++servo);
if (servo >= 125) updown = 0;
} else
{
Servo_SetAngle(AimServo, --servo);
if (servo <= 55)
{
updown = 1;
SweepCount++; // to keep track of when to rotate actual bot if we can't find the beacon in the current sector
if (SweepCount == 2)
{
RotateCW(45, 1);
TIM0_TIE &= ~_S12_C4I;
}
if (SweepCount == 4)
{
RotateCCW(90, 1);
TIM0_TIE &= ~_S12_C4I;
}
if (SweepCount == 6)
{
SweepCount = 0;
}
}
}
printf("Servo angle = %d\r", servo);
ES_Timer_SetTimer(5, 20);
ES_Timer_StartTimer(5);
// printf("Start Timer 5\r\n");
} else if (Event.EventType == Aligned)
{
//Adjustment for mechanical difference
if(updown == 1)
{
servo += 5;
Servo_SetAngle(AimServo, servo);
} else
{
servo += 5;
Servo_SetAngle(AimServo, servo);
}
} else
// do the 'during' function for this state
{
}
// return either Event, if you don't want to allow the lower level machine
// to remap the current event, or ReturnEvent if you do want to allow it.
return(ReturnEvent);
}
static ES_Event DuringWaitForServo( ES_Event Event)
{
ES_Event ReturnEvent = Event; // assmes no re-mapping or comsumption
if (Event.EventType == ES_ENTRY )
{
printf("In WaitForServo\r\n");
UncloakBot(); //Never implemented
Servo_SetAngle(AimServo, angleForPower);
ES_Timer_SetTimer(5, 1000);
ES_Timer_StartTimer(5);
}
else if ( Event.EventType == ES_EXIT )
{
}else
{
}
return(ReturnEvent);
}
static ES_Event DuringWaitForLoader( ES_Event Event)
{
ES_Event ReturnEvent = Event; // assmes no re-mapping or comsumption
if (Event.EventType == ES_ENTRY )
{
printf("In WaitForLoader\r\n");
printf("Load shooter\r\n");
loadShooter();
ES_Timer_SetTimer(2, 3000);//Timer especially long for increased PWM, used mainly for grading session
ES_Timer_StartTimer(2);
printf("Start Timer 2\r\n");
}
else if ( Event.EventType == ES_EXIT )
{
}else
{
}
return(ReturnEvent);
}
static ES_Event DuringShooting( ES_Event Event)
{
ES_Event ReturnEvent = Event; // assmes no re-mapping or comsumption
ES_Event currEvent;
if (Event.EventType == ES_ENTRY)
{
printf("In Shooting\r\n");
printf("Shoot ball\r\n");
shootBall();
ES_Timer_SetTimer(2,300);
ES_Timer_StartTimer(2);
printf("Start Timer 2\r\n");
}
else if ( Event.EventType == ES_EXIT )
{
} else
{
}
return(ReturnEvent);
}
void setShooterSpeed_PowerSM( void)
{
double Current_BatVoltage;
double ShooterMotorPWM;
double PowerStationDistance;
int ShooterAngle;
// static int FirstTime = 0;
int botangle;
if(getColor() ==2){ //blue
PowerStation.x = 0;
} else if (getColor()==1) { //red
PowerStation.x = FAC_MAX_X;
}
if (getColor() == 1) //red
{
botangle = 270-OurBot.r*45/32;
ShooterAngle = (servo-90);
PowerStation.y = OurBot.y - (OurBot.x - PowerStation.x)*tan((ShooterAngle + botangle)*3.14/180); // *tan(servo - ourbot.r)
} else // blue
{
botangle = 90- OurBot.r*45/32;//= 45/32*OurBot.r - 90;
ShooterAngle =(servo-90);
PowerStation.y = OurBot.y - (OurBot.x - PowerStation.x)*tan((ShooterAngle + botangle)*3.14/180); // *tan(servo - ourbot.r)
if(PowerStation.y > 255) PowerStation.y =255;
else if (PowerStation.y <0) PowerStation.y = 0;
}
printf("Powery %d ShooterAng %d botangle %d \r\n",PowerStation.y, (int)ShooterAngle, (int)botangle);
printf("Ourbot x %d y %d r %d\r\n", OurBot.x, OurBot.y, OurBot.r);
// PowerStation.y = OurBot.y - (OurBot.x - PowerStation.x)*tan(ShooterAngle + botangle); // *tan(servo - ourbot.r)
PowerStationDistance = (double)sqrt((pow((OurBot.y-PowerStation.y),2) + pow((OurBot.x-PowerStation.x),2))); //Max should be about 190
Current_BatVoltage = (double)ADS12_ReadADPin(4);
ShooterMotorPWM = (CAL_BAT_VOLTAGE/Current_BatVoltage)*(PowerStationDistance*SHOOTER_DISTANCE_SCALE_M + SHOOTER_DISTANCE_SCALE_B);
SetDuty((int)ShooterMotorPWM,1);
printf("ShooterMotorPWM %d PowerStationdistance %d PowerStationy %d OurBoty %d\r\n", (int)ShooterMotorPWM, (int)PowerStationDistance, PowerStation.y, OurBot.y);
}
/*******************************
Public Functions
*******************************/