void grabBall(void)
{
SetPin(PIN_F2, blink_on);
SetPin(PIN_F3, !blink_on);
//Open hand
SetPin(PIN_F3, blink_on);
SetPin(PIN_F3, !blink_on);
SetServo(handServo, 0.2f);
Wait(1.0);
lowerArm();
//Close hand
SetPin(PIN_F3, blink_on);
SetPin(PIN_F2, !blink_on);
SetServo(handServo, 0.65f);
Wait(1.0);
//arm goes up
SetPin(PIN_F2, blink_on);
SetPin(PIN_F3, !blink_on);
raiseArm();
}
/****************************************************************************
Function
RunLance
Parameters
ES_Event : the event to process
Returns
ES_Event, ES_NO_EVENT if no error ES_ERROR otherwise
Description
When a deploy lance event is recieved, the robot uses a servo to extend
the lance and a timer is used to wait the 3 seconds to retract lance
and 1 second wait until the lance is able to be extended again.
Author
P. Sherman, 03/10/14, 20:40
J. Edward Carryer, 01/15/12, 15:23
****************************************************************************/
ES_Event RunLance( ES_Event ThisEvent )
{
ES_Event ReturnEvent;
ReturnEvent.EventType = ES_NO_EVENT; // assume no errors
//Deploy Lance if event is posted and lance is in valid state
if(ThisEvent.EventType == Deploy_Lance && CurrentState == Retracted)
{
SetServo(LANCE_SERVO, DEPLOY_WIDTH);
ES_Timer_InitTimer(Lance_Timer, 3*ONE_SEC);
CurrentState = Deployed;
}
//Timeout Event recieved
else if(ThisEvent.EventType == ES_TIMEOUT)
{
if(CurrentState == Deployed)
{
SetServo(LANCE_SERVO, RETRACT_WIDTH);
ES_Timer_InitTimer(Lance_Timer, ONE_SEC);
CurrentState = Inactive;
} else if (CurrentState == Inactive)
{
CurrentState = Retracted;
}
}
return ReturnEvent;
}
void dl_parse_msg( void ) {
uint8_t msg_id = IdOfMsg(dl_buffer);
if (msg_id == DL_SET_ACTUATOR) {
uint8_t servo_no = DL_SET_ACTUATOR_no(dl_buffer);
uint16_t servo_value = DL_SET_ACTUATOR_value(dl_buffer);
LED_TOGGLE(2);
if (servo_no < SERVOS_NB)
SetServo(servo_no, servo_value);
}
#ifdef DlSetting
else if (msg_id == DL_SETTING && DL_SETTING_ac_id(dl_buffer) == AC_ID) {
uint8_t i = DL_SETTING_index(dl_buffer);
float val = DL_SETTING_value(dl_buffer);
DlSetting(i, val);
LED_TOGGLE(2);
for (int j=0 ; j<8 ; j++) {
SetServo(j,actuators[j]);
}
DOWNLINK_SEND_DL_VALUE(DefaultChannel, DefaultDevice, &i, &val);
} else if (msg_id == DL_GET_SETTING && DL_GET_SETTING_ac_id(dl_buffer) == AC_ID) {
uint8_t i = DL_GET_SETTING_index(dl_buffer);
float val = settings_get_value(i);
DOWNLINK_SEND_DL_VALUE(DefaultChannel, DefaultDevice, &i, &val);
}
#endif
}
void ServoInit()
{
//setup timer
T1TCR=0x3;
T1PR=234;
ServoFlag=1;
SetServo(0,128);
SetServo(1,128);
SetServo(2,128);
SetServo(3,128);
T1MCR=0x1;
T1MR0=ServoVal[0];
T1MR1=ServoVal[1];
T1MR2=ServoVal[2];
T1MR3=ServoVal[3];
T1EMR=0x0550;
T1IR=0xff;
PINSEL0|=0x0a000000;
PINSEL1|=0x0000010c;
//init interrupts
VICINTENABLE=0x20;
VICVECTCNTL5=0x20 | 5;
VICVECTADDR5=(UINT)ServoInt;
T1TCR=0x1;
}
void score(int k){
//if ir yes and ir no, drop flaps
setm(0.2f,0.2f);
Wait(1.0f);
setm(0.0f,0.0f);
if(k==0)
SetServo(marservo,0.5f);
else
SetServo(pingservo,0.5f);
Wait(2.0f);
SetServo(marservo,0.0f);
SetServo(pingservo,0.0f);
}
/* Initialize the turning servo with the TPM module.
* Sets up the timer clock and aligns servo to center
*/
void InitServo() {
SIM_SOPT2 |= SIM_SOPT2_PLLFLLSEL_MASK;
SIM_SOPT2 &= ~(SIM_SOPT2_TPMSRC_MASK);
SIM_SOPT2 |= SIM_SOPT2_TPMSRC(1);
SIM_SCGC6 |= SIM_SCGC6_TPM1_MASK;
TPM1_SC = 0;
TPM1_CONF = 0;
TPM1_SC = TPM_SC_PS(SERVO_CLK_PRESCALE);
TPM1_SC |= TPM_SC_TOIE_MASK;
TPM1_MOD = SERVO_CLK / (1 << (SERVO_CLK_PRESCALE + 1)) / SERVO_FREQ;
TPM1_C0SC = TPM_CnSC_MSB_MASK | TPM_CnSC_ELSB_MASK;
TPM1_C1SC = TPM_CnSC_MSB_MASK | TPM_CnSC_ELSB_MASK;
SetServo(0.0);
TPM1_SC |= TPM_SC_CMOD(1);
enable_irq(INT_TPM1 - 16);
PORTB_PCR0 = PORT_PCR_MUX(3);
}
void CdynControlDlg::OnLbnDblclkList1()
{
// TODO: Add your control notification handler code here
int idx = m_predefined.GetCurSel();
CString str;
m_predefined.GetText(idx, str);
str = _T("Run: ") + str;
if (MessageBox(str, NULL, MB_OKCANCEL | MB_ICONWARNING) != IDOK)
return;
cJSON *action = cJSON_GetObjectItem(m_actionJSON, actions[idx]);
int num = cJSON_GetArraySize(action);
log(_T("\n-- start action %s with %d steps --\n"), str, num);
for (int i = 0; i < num; i++) {
cJSON *cmd = cJSON_GetArrayItem(action, i);
SetServo(cJSON_GetObjectItem(cmd, "id")->valueint,
cJSON_GetObjectItem(cmd, "deg")->valueint);
SendServo(cJSON_GetObjectItem(cmd, "id")->valueint);
Sleep(1000);
}
log(_T("-- end action --\n\n"), str, num);
}
void CdynControlDlg::ResetAll()
{
cJSON *config;
for (int i = 0; i < cJSON_GetArraySize(m_servoJSON); i++) {
config = cJSON_GetArrayItem(m_servoJSON, i);
SetServo(cJSON_GetObjectItem(config, "id")->valueint,
cJSON_GetObjectItem(config, "init")->valueint);
}
}
void main(void) {
InitPLL();
InitServo();
StartServo();
SetServo(90);
StartMenu();
FOREVER();
}
void dropBall(void)
{
//move arm up halfway
SetPin(PIN_F2, blink_on);
SetPin(PIN_F3, !blink_on);
raiseArmPeak();
//open hand and wait for ball to drop
SetPin(PIN_F3, blink_on);
SetPin(PIN_F3, !blink_on);
SetServo(handServo, 0.2f);
Wait(3.0);
//close hand
SetPin(PIN_F3, blink_on);
SetPin(PIN_F2, !blink_on);
SetServo(handServo, 0.65f);
Wait(2.0);
//arm goes down
raiseArm();
}
void PID_UpdateCentroid(PID * pid)
{
int P, I, D;
float refreshRate;
//------Read Encoder and clock
uint32 nowClocks = ClockTime();
//------Time since last function call in seconds
refreshRate = refresh_rate(nowClocks, pid->lastClockTicks_c);
pid->lastClockTicks_c = nowClocks;
//------Calculate Error
pid->error_c = pid->desiredCentroidPID - pid->currentCentroid;
///Wireless_ControlLog(pid->currentCentroid, pid->desiredCentroidPID);
//------Update Derivative
//pid->differentiator_c = ((((2*pid->Tau)-refreshRate)/((2*pid->Tau)+refreshRate))*pid->differentiator_c) + ((2/((2*pid->Tau)+refreshRate))*(pid->currentCentroid - pid->lastCurrentCentroid));
pid->differentiator_c = ((((2*pid->Tau)-refreshRate)/((2*pid->Tau)+refreshRate))*pid->differentiator_c) + ((2/((2*pid->Tau)+refreshRate))*(pid->error_c - pid->lastError_c));
//------Update integrator - AntiWindup(only use the integrator if we are close, but not too close)
if( abs(pid->error_c) < 75)
pid->integrator_c = pid->integrator_c + ((refreshRate/2)*(pid->error_c + pid->lastError_c))*abs(pid->currentVelocity)/1500;
else
pid->integrator_c = 0;
//------Scale Kp based on current velocity
float Kp = pid->Kp_c * 1000 / abs(pid->currentVelocity);
//------Output Calculation
P = Kp * pid->error_c;
I = pid->Ki_c * pid->integrator_c;
D = pid->Kd_c * pid->differentiator_c;
pid->outputPID_unsat_c = (P) + (I) - (D);
pid->outputPID_c = sat(pid->outputPID_unsat_c, 40);
//pid->integrator_c = pid->integrator_c + (refreshRate/pid->Ki_c)*(pid->outputPID_c - pid->outputPID_unsat_c);
//------Save states and send PWM to motors
pid->lastCurrentCentroid = pid->currentCentroid;
pid->lastError_c = pid->error_c;
//------Condition output on current velocity
if (pid->currentVelocity < 0)
pid->outputPID_c = -pid->outputPID_c;
if (pid->currentVelocity > -20 && pid->currentVelocity < 20)
pid->outputPID_c = 0;
if (pid->outputPID_c == 0)
pid->outputPID_c = 0;
SetServo(RC_STR_SERVO, pid->outputPID_c);
}
void ServoDriverLininoSetServos(UINT32 servoc, UINT32 *servov)
{
if(!isInit)
{
InitServos();
isInit=TRUE;
}
UINT s=0;
BOOL servoChanged = FALSE;
static UINT count;
UINT update = (count++ % 50);
for(s=0; s < MAX_SERVOS; s++)
{
if(s < servoc )
{
// update channel if position changed or 1s interval if servo is not disabled
if((oldChannels[s] != servov[s]) || ((update == s) && servov[s]))
{
SetServo(s, servov[s]);
oldChannels[s] = servov[s];
servoChanged = TRUE;
}
}
else
{
if(oldChannels[s] != 0)
{
SetServo(s, 0);
oldChannels[s] = 0;
servoChanged = TRUE;
}
}
}
if(servoChanged)
PrintServoStates();
}
//Initializes motors and IR Sebsors
void initMotorsSensors(void) {
if (!initialized) {
initialized = true;
Motors[0] = InitializeServoMotor(PIN_B6, false);
Motors[1] = InitializeServoMotor(PIN_B7, false);
Motors[2] = InitializeServoMotor(PIN_C4, false);
Motors[3] = InitializeServoMotor(PIN_C5, false);
marservo = InitializeServo(PIN_B2);
pingservo = InitializeServo(PIN_F3);
SetServo(marservo,0.1f);
SetServo(pingservo,0.1f);
GPIOPadConfigSet(PORT_VAL(PIN_B6), PIN_VAL(PIN_B6), GPIO_STRENGTH_8MA, GPIO_PIN_TYPE_STD);
GPIOPadConfigSet(PORT_VAL(PIN_B7), PIN_VAL(PIN_B7), GPIO_STRENGTH_8MA, GPIO_PIN_TYPE_STD);
GPIOPadConfigSet(PORT_VAL(PIN_C4), PIN_VAL(PIN_C4), GPIO_STRENGTH_8MA, GPIO_PIN_TYPE_STD);
GPIOPadConfigSet(PORT_VAL(PIN_C5), PIN_VAL(PIN_C5), GPIO_STRENGTH_8MA, GPIO_PIN_TYPE_STD);
adc[0] = InitializeADC(PIN_D0);
adc[1] = InitializeADC(PIN_D1);
adc[2] = InitializeADC(PIN_D2);
adc[3] = InitializeADC(PIN_D3);
turn=0;
gls = InitializeGPIOLineSensor(
PIN_B5,
PIN_B0,
PIN_B1,
PIN_E4,
PIN_E5,
PIN_B4,
PIN_A5,
PIN_A6
);
}
}
void init_fbw( void ) {
mcu_init();
actuators_init();
uint8_t i;
for(i = 0; i < SERVOS_NB; i++) {
SetServo(i, 1500);
}
// SetServo(SERVO_GAZ, SERVO_GAZ_MIN);
sys_time_register_timer((1./PERIODIC_FREQUENCY), NULL);
mcu_int_enable();
}
int main(void) {
initIRSensor();
initMotor();
initGPIOLineSensor();
initServo();
SetServo(servo,0);
stage = 0;
//while(1){followLine();}
//while(1){followWall();}
//SetMotor(leftMotor, 1); SetMotor(rightMotor, 1);
while(true){
LineSensorReadArray(gls, line);
if (stage==0){ //start state
if(line[0]<0.5&&line[1]<0.5&&line[2]<0.5&&line[3]<0.5&&line[4]<0.5&&line[5]<0.5&&line[6]<0.5&&line[7]<0.5) {
followWall();
}else{
followLine();
}
}else if(stage==1){
//once 90degree turn passed
// SetMotor(leftMotor, 1);
// SetMotor(rightMotor, -1);
SetPin(PIN_F2, 1);
followWall();
if (wallPresent()) {followWall();}
else {
findLine();
}
SetPin(PIN_F2, 0);
}else if (stage==2){
//once line found again after walled section
SetPin(PIN_F1, 1);
if((line[0]<0.5&&line[1]<0.5&&line[2]<0.5&&line[3]<0.5&&line[4]<0.5&&line[5]<0.5&&line[6]<0.5&&line[7]<0.5)||
(mostDark())) { //line[0]>0.5&&line[1]>0.5&&line[2]>0.5&&line[3]>0.5&&line[4]>0.5&&line[5]>0.5&&line[6]>0.5&&line[7]>0.5)
findEnd();
}else{
followLine();
};
SetPin(PIN_F1, 0);
}else{//end of course look for flag
findObject();
break;
}
}
}
void StartCore(void) {
PORTB = 0xAA;
DDRB = 0xFF;
// 初始化小按键
DDRP_DDRP0 = 0;
PPSP_PPSP0 = 0;
PERP_PERP0 = 1;
/********************************/
/* 初始化 */
InitServo();
StartServo();
SetServo(90);
InitSpeeder();
irInit();
InitADC();
InitSCI0();
WaitEnable();
/*******************************/
GetBlackAndWhite();
PORTB = 0x55;
Wait(1500);
PORTB = 0xAA;
while (PTIP_PTIP0);
PORTB = 0x5A;
Wait(1000);
PORTB = 0xA5;
CoreControl();
}
int MotorCompliance(void)
{
int StopSwitch = 0;
int ForLoop = 1;
int ArmEmergencyRelease = 0;
unsigned int LetsGo = 0;
/* Bring the motor down at first powerup so the robot is compliant. */
while(ForLoop) // This is the same while loop as in the main program,
{ // but is using a variable set that starts set to 1
/* Get digital input from the switch mounted to the tower.
* GetDigitalInput is weird - 1 is unpressed, 0 is pressed */
StopSwitch = GetDigitalInput(7);
if(StopSwitch == 1)
{
/* Button on the joypad to press in case the above DI doesn't trigger.
* Same button as the arm unlock button. */
ArmEmergencyRelease = GetJoystickDigital(1,5,2);
if(ArmEmergencyRelease == 1)
{
SetServo(6,0);
ForLoop = 0;
}
/* Set various motors and servos to a compliant position */
SetMotor(2,-90);
SetServo(4,64);
SetServo(3,127);
SetServo(6,127);
SetServo(7,127);
}
else
{
LetsGo = GetJoystickAnalog(1,3);
if(LetsGo) //Move Raisy in any position where it won't be set to 0
{
SetServo(6,0); //Set Tilty to the straight position
ForLoop = 0; //Exit the while loop
}
else
{
SetMotor(2,-40); // Keep Raisy down to the base until the joystick is moved
}
}
}
/* Return 1 when this function is called. We need this set to 1 so this init function only runs
* once. MotorBringDown gets set to 1 and thus fails the if check when we do this. */
return 1;
}
void setSteeringRadius(int direction, uint32 radius_cm) {
//print("start radius\r\n");
int str = 0;
switch (radius_cm){
case 100:
str = 64;
break;
case 125:
str = 56;
break;
case 150:
str = 48;
break;
case 175:
str = 44;
break;
case 200:
str = 40;
break;
case 225:
str = 36;
break;
case 250:
str = 32;
break;
case 275:
str = 28;
break;
case 300:
str = 24;
break;
default:
str = 0;
break;
//print("end radius\r\n");
}
//print("servo\r\n");
SetServo(RC_STR_SERVO, (direction*str));
}
void updateVelocityOutput()
{
float desiredVelocity = pid.desiredVelocityPID;
float P, I, D, currentVelocity;
uint32 deltaClocks;
CPU_MSR msr;
//------Read Encoder and clock
msr = DISABLE_INTERRUPTS();
pid.encoderValue = getTicks();
uint32 nowClocks = ClockTime();
RESTORE_INTERRUPTS(msr);
//------Time since last function call in seconds
uint32 maxClocks = 0xffffffff;
if ((nowClocks < pid.lastClockTicks))
deltaClocks = (maxClocks-pid.lastClockTicks)+nowClocks;
else
deltaClocks = nowClocks - pid.lastClockTicks;
float refreshRate = ((float)deltaClocks)/XPAR_CPU_PPC405_CORE_CLOCK_FREQ_HZ ;//time passed since last function call: sec
//uint32 refreshRate = (deltaClocks)/(XPAR_CPU_PPC405_CORE_CLOCK_FREQ_HZ/1000) ;
//xil_printf("nowClock: %d\r\n", nowClocks);
//xil_printf("pid.lastClockTicks: %d\r\n", pid.lastClockTicks);
//xil_printf("Delta Clock: %d\r\n", deltaClocks);
//xil_printf("Refresh Rate: ");
//PrintFloat(refreshRate);
//print("\n\r");
pid.lastClockTicks = nowClocks;
//------Distance since last function call in ticks
int encoderDifference = pid.encoderValue - pid.lastEncoderValue;
//xil_printf("Encoder Value: %d\r\n", pid.encoderValue);
//xil_printf("Last Encoder Value: %d\r\n", pid.lastEncoderValue);
//xil_printf("Encoder Diff: %d\r\n", encoderDifference);
//------Calculate velocity
currentVelocity = ((encoderDifference) / (refreshRate));
//xil_printf("Current Velocity: ");
PrintFloat(currentVelocity);
xil_printf(",");
//xil_printf("Desired Velocity: ");
PrintFloat(desiredVelocity);
xil_printf(",");
//print("\n\r");
//------Error
pid.error = desiredVelocity - (currentVelocity);
// xil_printf("error: ");
// PrintFloat(pid.error);
// print("\n\r");
//------Update Derivative
pid.differentiator = (2*pid.Tau-refreshRate)/(2*pid.Tau+refreshRate)*pid.differentiator + 2/(2*pid.Tau+refreshRate)*(pid.error-pid.lastError);
//xil_printf("differentiator: ");
//PrintFloat(pid.differentiator);
//print("\n\r");
//------Update integrator - AntiWindup(only use the integrator if we are close, but not too close)
if ((pid.error < 1500 && pid.error > -1500) && (pid.error > 10 || pid.error < -10)){
pid.integrator = pid.integrator + (refreshRate/2)*(pid.error + pid.lastError);
} else {
pid.integrator = 0;
}
// if(pid.error == 0) {
// pid.integrator = 0;
// }
// xil_printf("integrator: ");
// PrintFloat(pid.integrator);
// print("\n\r");
//------Output Calculation
P = pid.Kp * pid.error;
I = pid.Ki * pid.integrator;
D = pid.Kd * 0;//pid.differentiator;
pid.outputPID_unsat = P + I - D;
pid.outputPID = sat(pid.outputPID_unsat, 60);
//------Save states and send PWM to motors
pid.lastEncoderValue = pid.encoderValue;
pid.lastCurrentVelocity = currentVelocity;
pid.lastDesiredVelocity = desiredVelocity;
SetServo(RC_VEL_SERVO, (int)pid.outputPID);
// xil_printf("Output PID unsat: ");
// PrintFloat(pid.outputPID_unsat);
// print("\n\r");
// xil_printf("Output PID: %d", pid.outputPID);
// print("--------------------------------------\r\n");
}
void PID_UpdateRadius(PID * pid)
{
int P, I, D;
double refreshRate;
CPU_MSR msr;
//------Read Encoder and clock
msr = DISABLE_INTERRUPTS();
float desiredRadius = pid->desiredRadiusPID;//----//get info from camera here
uint32 nowClocks = ClockTime();
// Gyro_Calculation(pid->gyro);
RESTORE_INTERRUPTS(msr);
//------Time since last function call in seconds
refreshRate = refresh_rate(nowClocks, pid->lastClockTicks_r);
pid->lastClockTicks_r = nowClocks;
//------Current Radius at front of car
pid->currentRadius = pid->gyro->frontRadius;
if (pid->currentRadius > 6000)
pid->currentRadius = 6000;
else if (pid->currentRadius < -6000)
pid->currentRadius = -6000;
//------Calculate Error
pid->error_r = ((float)(pid->currentRadius - desiredRadius))/(pid->currentRadius * desiredRadius);
//------Update Derivative
pid->differentiator_r = ((((2*pid->Tau)-refreshRate)/((2*pid->Tau)+refreshRate))*pid->differentiator_r) + ((2/((2*pid->Tau)+refreshRate))*(pid->currentRadius - pid->lastCurrentRadius));
//------Update integrator - AntiWindup(only use the integrator if we are close, but not too close)
if (abs(((int)(1000000*pid->error_r))) > 200)
pid->integrator_r = ((int)((pid->integrator_r))) + ((int)(1000000*((refreshRate/2)*(pid->error_r + pid->lastError_r))));
//------Output Calculation
//pid->Ki_r = pid->Ki_r/1000000;
P = (pid->Kp_r * pid->error_r);
I = (pid->Ki_r * pid->integrator_r);
D = (pid->Kd_r * pid->differentiator_r);
pid->radiusEqualibrium = (40000.0/desiredRadius);
// Wireless_Debug("P: ");
// PrintInt(P);
// Wireless_Debug("\n\r");
// Wireless_Debug("Integral: ");
// PrintFloat(pid->integrator_r);
// Wireless_Debug("\n\r");
// Wireless_Debug("error: ");
// PrintFloat(pid->error_r);
// Wireless_Debug("\n\r");
// Wireless_Debug("lastError: ");
// PrintFloat(pid->lastError_r);
// Wireless_Debug("\n\r");
// Wireless_Debug("Refresh: ");
// PrintFloat(refreshRate);
// Wireless_Debug("\n\r");
// Wireless_Debug("I: ");
// PrintInt(I);
// Wireless_Debug("\n\r");
// Wireless_Debug("velocityBack: ");
// PrintFloat(pid->currentVelocityBack);
// Wireless_Debug("\n\r");
// Wireless_Debug("velocityFront: ");
// PrintFloat(pid->currentVelocity);
// Wireless_Debug("\n\r");
// Wireless_Debug("-----------------------------");
// Wireless_Debug("\n\r");
// Wireless_Debug("D: ");
// PrintInt(D);
// Wireless_Debug("\n\r");
// Wireless_ControlLog_Ext(pid->currentRadius, pid->desiredRadiusPID, pid->error, pid->outputPID_unsat, P);
//pid->outputPID_unsat_r = (P + I - D) + pid->radiusEqualibrium;
pid->outputPID_unsat_r = pid->radiusEqualibrium;
pid->outputPID_r = sat(pid->outputPID_unsat_r, 40);
//pid->integrator_r = ((int)pid->integrator_r) + ((int)(1000000*(refreshRate/pid->Ki_r)*(pid->outputPID_r - pid->outputPID_unsat_r)));
//------Save Info for graph
// Wireless_ControlLog_Ext(pid->currentRadius, desiredRadius, pid->error_r, pid->outputPID_unsat_r, I);
//------Save states and send PWM to motors
pid->lastCurrentRadius = pid->currentRadius;
pid->lastError_r = pid->error_r;
pid->lastIntegrator_r = pid->integrator_r;
// Wireless_Debug("Last Error: ");
// PrintFloat(pid->lastError_r);
// Wireless_Debug("\n\r");
// Wireless_Debug("Integral: ");
// PrintFloat(pid->integrator_r);
// Wireless_Debug("\n\r");
// Wireless_Debug("-----------------------------");
// Wireless_Debug("\n\r");
SetServo(RC_STR_SERVO, pid->outputPID_r);
}
void PID_UpdateVelocity(PID * pid)
{
int desiredVelocity = pid->desiredVelocityPID;
float P, I, D;
float refreshRate;
CPU_MSR msr;
//------Read Encoder and clock
msr = DISABLE_INTERRUPTS();
pid->encoderValue = getTicks();
uint32 nowClocks = ClockTime();
Gyro_Calculation(pid->gyro);
RESTORE_INTERRUPTS(msr);
//------Time since last function call in seconds
refreshRate = refresh_rate(nowClocks, pid->lastClockTicks);
pid->lastClockTicks = nowClocks;
//------Distance since last function call in ticks
int encoderDifference = pid->encoderValue - pid->lastEncoderValue;
//------Calculate Velocity
pid->currentVelocity = (int)((encoderDifference) / (refreshRate));
//pid->currentVelocityBack = (int)((encoderDifference) / (refreshRate));
//------Convert Back Velocity to front
//pid->currentVelocity = Gyro_VelocityBackToFront(pid->gyro,pid->currentVelocityBack);
//------Calculate Error
pid->error = desiredVelocity - (pid->currentVelocity);
//------Update Derivative
//pid->differentiator = ((((2*pid->Tau)-refreshRate)/((2*pid->Tau)+refreshRate))*pid->differentiator) + ((2/((2*pid->Tau)+refreshRate))*(pid->currentVelocity - pid->lastCurrentVelocity));
pid->differentiator = ((((2*pid->Tau)-refreshRate)/((2*pid->Tau)+refreshRate))*pid->differentiator) + ((2/((2*pid->Tau)+refreshRate))*(pid->error - pid->lastError));
//------Update integrator - AntiWindup(only use the integrator if we are close, but not too close)
//If we have switched directions, zero the integrator
if (abs(pid->lastDesiredVelocity) / pid->lastDesiredVelocity !=
abs(pid->desiredVelocityPID) / pid->desiredVelocityPID) {
pid->integrator = 0;
} else {
pid->integrator = pid->integrator + ((refreshRate/2)*(pid->error + pid->lastError));
}
//------Output Calculation
P = pid->Kp * pid->error;
I = pid->Ki * pid->integrator;
D = pid->Kd * pid->differentiator;
pid->outputPID_unsat = ((int)P) + ((int)I) - ((int)D);
pid->outputPID = sat(pid->outputPID_unsat, 40);
//pid->outputPID = downShift(pid->error, -600, 7);
pid->integrator = pid->integrator + (refreshRate/pid->Ki)*(pid->outputPID - pid->outputPID_unsat);
//Wireless_ControlLog_Ext(pid->currentVelocity, desiredVelocity, pid->error, pid->outputPID_unsat, P);
//------Save states and send PWM to motors
pid->lastEncoderValue = pid->encoderValue;
pid->lastCurrentVelocity = pid->currentVelocity;
pid->lastDesiredVelocity = desiredVelocity;
SetServo(RC_VEL_SERVO, (int)pid->outputPID);
}
/*-------------------------------------------------------------------
* UserMain
*-----------------------------------------------------------------*/
void UserMain(void *pd)
{
SimpleUart(0,115200);
assign_stdio(0);
iprintf("Before init stack\r\n");
InitializeStack();
{
WORD ncounts = 0;
while ( ( !bEtherLink ) && ( ncounts < 2*TICKS_PER_SECOND ) )
{
ncounts++;
OSTimeDly( 1 );
}
}
EnableAutoUpdate();
EnableTaskMonitor();
OSChangePrio( MAIN_PRIO ); // set standard UserMain task priority
//BcastSysLogPrintf("Application built on %s on %s\r\nWait...", __TIME__, __DATE__ );
pUserUdpProcessFunction=MyUDPProcessFunction;
// UdpCheck();
OSSemInit(&DataSem,0);
LoadSensorConfig();
// BcastSysLogPrintf("1");
InitLog();
ImuInit(IMU_PRIO,&DataSem);
InitIDSM2SubSystem(RC_PRIO,&DataSem);
InitGpsSubSystem(GPS_PRIO,&DataSem);
InitServos();
SetServo(0,0);
SetServo(1,0);
SetServo(2,0);
SetServo(3,0);
WORD LastGps=GPS_Result.ReadingNum;
WORD LastRc =DSM2_Result.ReadingNum;
WORD LastImu =IMU_Result.ReadingNum;
DWORD LSecs=Secs;
static short mmx;
static short mmy;
static short nmx;
static short nmy;
static short mgz;
static short ngz;
DWORD ZeroTime=Secs+5;
long GZSum=0;
int GZCnt=0;
float fmh=0.0;
while(ZeroTime > Secs)
{
OSSemPend(&DataSem,20);
if(LastImu !=IMU_Result.ReadingNum)
{
GZSum+=IMU_Result.gz;
GZCnt++;
LastImu =IMU_Result.ReadingNum;
if (IMU_Result.mx !=0 )
fmh=CalcMagHeading(IMU_Result.mx,IMU_Result.my);
}
}
short gzoff=(GZSum/GZCnt);
float fIheading=fmh;
while(1)
{
OSSemPend(&DataSem,20);
if (LastGps!=GPS_Result.ReadingNum )
{
LastGps=GPS_Result.ReadingNum;
LogSmGps(GPS_Result);
}
if (LastRc !=DSM2_Result.ReadingNum)
{
WORD w=DSM2_Result.val[1];
float f=DSM_Con(w);
SetServo(STEER_CH,f);
w=DSM2_Result.val[0];
f=DSM_Con(w);
SetServo(THROTTLE_CH,f);
LastRc =DSM2_Result.ReadingNum;
// LogRC(DSM2_Result);
}
if(LSecs!=Secs)
{
// BcastSysLogPrintf("Tick %d Iat:%ld lon:%ld SAT:%d\r\n",Secs,GPS_Result.LAT,GPS_Result.LON,GPS_Result.numSV);
//SysLogPrintf(ipa_syslog_addr,514,
LogMaxMin(mgz,mmx,mmy,ngz,nmx,nmy);
//static char tbuf[256];
// siprintf(tbuf,"TCN=%ld\r\n",sim.timer[0].tcn);
// writestring(LOG_UART,tbuf);
mgz=IMU_Result.mz;
mmx=IMU_Result.mx;
mmy=IMU_Result.my;
ngz=IMU_Result.mz;
nmx=IMU_Result.mx;
nmy=IMU_Result.my;
LSecs=Secs;
}
if(LastImu !=IMU_Result.ReadingNum)
{
if(IMU_Result.gz<ngz) ngz=IMU_Result.gz;
if(IMU_Result.gz>mgz) mgz=IMU_Result.gz;
fIheading+=(fGZHeadingScale_deg*(IMU_Result.gz-gzoff));
if(fIheading > 180.0 ) fIheading-=360.0;
if(fIheading <-180.0 ) fIheading+=360.0;
if (IMU_Result.mx !=0 )
{
if(IMU_Result.mx<nmx) nmx=IMU_Result.mx;
if(IMU_Result.mx>mmx) mmx=IMU_Result.mx;
if(IMU_Result.my<nmy) nmy=IMU_Result.my;
if(IMU_Result.my>mmy) mmy=IMU_Result.my;
volatile ImuRegisters il;
OSLock();
il.ax=IMU_Result.ax;
il.ay=IMU_Result.ay;
il.az=IMU_Result.az;
il.gx=IMU_Result.gx;
il.gy=IMU_Result.gy;
il.gz=IMU_Result.gz;
il.mx=IMU_Result.mx;
il.my=IMU_Result.my;
il.mz=IMU_Result.mz;
il.t =IMU_Result.t ;
il.ReadingNum=IMU_Result.ReadingNum;
OSUnlock();
fmh=CalcMagHeading(IMU_Result.mx,IMU_Result.my);
CorrectHeading(fIheading,fmh,0.005);//20 times per second correct in 10 seconds so 0.005
il.fIhead=fIheading;
il.fMhead=fmh;
il.GHeading= GPS_Result.Heading;
il.odo=sim.timer[0].tcn;
LogImu(il);
}
LastImu =IMU_Result.ReadingNum;
}
}//While
}
void tryservo(void){
Wait(2.0f);
SetServo(marservo,0.5f);
Wait(2.0f);
SetServo(marservo,0.1f);
}
void main(void)
{
int Servo1 = 0;
int ServoFlagBits = 0;
int Servo2Test = 0;
int Openy1 = 0;
int Openy2 = 0;
int Openy3 = 0;
int Tilty1 = 0;
int Tilty2 = 0;
int Tilty3 = 0;
int Tilty4 = 0;
int Flippy1 = 0;
int Flippy2 = 0;
int Flippy3 = 0;
int Flippy4 = 0;
int MotorStop = 0;
int ProgramGo = 0;
int MotorBringDown = 0;
unsigned int ServoCount = 0;
unsigned int MotorPos = 0;
while(1)
{
/* Call the function that brings the arm down for compliance. */
if(MotorBringDown == 0) /* Run if variable is 0 (false) */
{
/* This call runs the function and puts the return value (1) into a variable.
* Since the above if statement checks if MotorBringDown is equal to 0 and we
* set it to 1, this call never runs again. */
MotorBringDown = MotorCompliance();
}
/* Motor Lock buttons
*
* Any GetJoystickDigital calls return 1 if pressed or 0 if not pressed. */
ProgramGo = GetJoystickDigital(1,5,1); //Channel 5, Button 1 (DOWN)
if(ProgramGo == 1)
{
MotorStop = 1; //Block all Raisy activity until this is set to 0
}
if(MotorStop == 1)
{
/* Run the motor at a small negative value so it stays in place.
* this works because of how the arm was balanced in 2013 */
SetMotor(2,-30);
/* Checking here to see if we need to unlock Raisy */
ProgramGo = GetJoystickDigital(1,5,2);
if(ProgramGo == 1) //If so...
{
MotorStop = 0; //...unlock it
}
}
else
{
/* Get the value of Joystick channel 3 and return it to an unsigned
* int since the joysticks range from -127 to 127 */
MotorPos = GetJoystickAnalog(1,3);
if(MotorPos > 10)
{
if(MotorPos < 50)
{
SetMotor(2,30); //Slow down the motor if MotorPos is greater than 50
}
else
{
SetMotor(2,MotorPos); //Set the motor speed to whatever is in MotorPos
}
}
else
{
SetMotor(2,-30); //Freeze Raisy if MotorPos is less than 10
}
}
/* Extendy. Plain and simple. */
JoystickToMotor(1,2,5,0);
/* Code for Openy */
Openy1 = GetJoystickDigital(1,6,1);
Openy3 = GetJoystickDigital(1,6,2);
if(Openy1 == 1)
{
SetServo(7,127);
Openy2 = 1;
}
else
{
if(Openy2 == 1 && Openy3 == 1)
{
SetServo(7,-127);
Openy2 = 0;
}
}
/* Code for Tilty */
Tilty1 = GetJoystickDigital(1,7,2);
Tilty2 = GetJoystickDigital(1,7,3);
Tilty3 = GetJoystickDigital(1,7,4);
if(Tilty1 == 1)
{
SetServo(6,0);
Tilty4 = 0;
}
else if(Tilty2 == 1)
{
SetServo(6,127);
Tilty4 = 1;
}
else if(Tilty3 == 1)
{
SetServo(6,-127);
Tilty4 = 1;
}
else
{
}
/* Code for Flippy */
Flippy1 = GetJoystickDigital(1,8,2);
Flippy2 = GetJoystickDigital(1,8,3);
Flippy3 = GetJoystickDigital(1,8,4);
if(Flippy1 == 1)
{
SetServo(3,-127);
SetServo(4,-64);
Flippy4 = 1;
}
else if(Flippy2 == 1)
{
SetServo(3,0);
SetServo(4,0);
Flippy4 = 1;
}
else if(Flippy3 == 1)
{
SetServo(3,127);
SetServo(4,64);
Flippy4 = 1;
}
/* Unfinished test code. Do not use. */
//Servo1 = GetJoystickDigital(1,8,2);
if(ServoFlagBits == 0)
{
if(Servo1 == 1)
{
for(ServoCount = 0; ServoCount < 128; ServoCount++)
{
//SetServo(9,ServoCount);
//Wait(5);
}
//ServoFlagBits = 1;
}
}
//Servo2Test = GetJoystickDigital(1,8,1);
if(ServoFlagBits == 1)
{
if(Servo2Test == 1)
{
/*
* for(ServoCount = 128; ServoCount --> 0;)
* {
* SetServo(9,ServoCount);
* }
* ServoFlagBits = 0;
*/
}
}
}
//Servo1 = 0;
}
void updateDistanceOutput()
{
int32 desiredDistance = pid.desiredDistancePID;
int32 P, I, D;
uint32 deltaClocks;
CPU_MSR msr;
msr = DISABLE_INTERRUPTS();
pid.encoderValue = getTicks();
uint32 nowClocks = ClockTime();
RESTORE_INTERRUPTS(msr);
//logData(desiredDistance, pid.encoderValue);
//------Time since last function call in seconds
uint32 maxClocks = 0xffffffff;
if ((nowClocks < pid.lastClockTicks))
deltaClocks = (maxClocks-pid.lastClockTicks)+nowClocks;
else
deltaClocks = nowClocks - pid.lastClockTicks;
float deltaTime = ((float)deltaClocks)/XPAR_CPU_PPC405_CORE_CLOCK_FREQ_HZ ;//time passed since last function call: sec
pid.error_d = desiredDistance - pid.encoderValue;
//
// xil_printf("Encoder Value: %d\r\n", pid.encoderValue);
// xil_printf("Desired Distance: %d\r\n", desiredDistance);
// xil_printf("error_d: %d\r\n", pid.error_d);
//------Update integrator - AntiWindup(only use the integrator if we are close, but not too close)
if ((pid.error_d < 1000 && pid.error_d > -1000) && (pid.error_d > 100 || pid.error_d < -100)){
pid.integrator_d = pid.integrator_d + (deltaTime/2)*(pid.error_d + pid.lastError_d);
}
else
pid.integrator_d = 0;
//------only use the integrator if we are close, but not too close
// if (pid.error_d < 500 && pid.error_d > -500 &&
// (pid.error_d > 100 || pid.error_d < -100))
// pid.integrator_d += pid.error_d * deltaTime;
// else
// pid.integrator_d = 0;
//print("integrator: ");
//PrintFloat(pid.integrator_d);
//print("\n\r");
//------Update Derivative
pid.differentiator_d = (2*pid.Tau-deltaTime)/(2*pid.Tau+deltaTime)*pid.differentiator_d + 2/(2*pid.Tau+deltaTime)*(pid.error_d-pid.lastError_d);
//print("differentiator: ");
//PrintFloat(pid.differentiator_d);
P = pid.Kp_d * pid.error_d;
I = pid.Ki_d * pid.integrator_d;
D = pid.Kd_d * pid.differentiator_d;
//print("P: ");
//PrintFloat(P);
//print("\n\r");
//print("I: ");
//PrintFloat(I);
//print("\n\r");
//print("D: ");
//PrintFloat(D);
//print("\n\r");
pid.outputPID_unsat = P + I - D;
pid.outputPID = sat(pid.outputPID_unsat, 60);
//------if we are really close, don't do anything!
if (pid.error_d < 100 && pid.error_d > -100) {
pid.outputPID = 0;
}
//------Integrator Anti-windup
// if (pid.Ki_d != 0.0f) {
// pid.integrator_d = pid.integrator_d + deltaTime/pid.Ki_d * (pid.outputPID - pid.outputPID_unsat);
// }
// print("integrator_antiWind: ");
// PrintFloat(pid.integrator_d);
//print("\n\r");
pid.lastError_d = pid.error_d;
pid.lastDesiredDistance = desiredDistance;
SetServo(RC_VEL_SERVO, pid.outputPID);
//xil_printf("Output PID: %d\r\n", pid.outputPID);
//print("--------------------------------------\r\n");
}
void servo_switch_periodic(void) {
if (servo_switch_on == TRUE)
SetServo(SERVO_SWITCH_SERVO, SERVO_SWITCH_ON_VALUE)
else
SetServo(SERVO_SWITCH_SERVO, SERVO_SWITCH_OFF_VALUE)
}
void findObject(void) {
int item = 0;
int close =0;
if (wasLeftWall){
SetMotor(rightMotor, -.1); //rotate
SetMotor(leftMotor, .1);
}else{
SetMotor(rightMotor, .1); //rotate
SetMotor(leftMotor, -.1);
}
while (item == 0) {
frontSensor = ADCRead(adc[0])*1000;
if (frontSensor > 300) {
item = 1;
}
}
SetServo(servo,1);
SetMotor(leftMotor, 0);
SetMotor(rightMotor, 0);
SysTick_Wait10ms(40);
SetMotor(leftMotor, 1);
SetMotor(rightMotor, 1);
//SysTick_Wait10ms(100);
// while (close == 0) {
// frontSensor = ADCRead(adc[0])*1000;
// if (frontSensor<300) {
// SetMotor(rightMotor, -1);
// SetMotor(leftMotor, 0);
// SysTick_Wait10ms(50);
// frontSensor = ADCRead(adc[0])*1000;
// SetMotor(rightMotor, 0.1);
// while(frontSensor<300){
// frontSensor = ADCRead(adc[0])*1000;
// }
// }
// else if(frontSensor>300&&frontSensor<850) {
// SetMotor(rightMotor, 1);
// SetMotor(leftMotor, 1);
// }
// else if(frontSensor>850) {
// close = 1;
// }
// }
// if (frontSensor<400){
// SysTick_Wait10ms(200);
// }
// else if (frontSensor<600){
// SysTick_Wait10ms(150);
// }
// else if (frontSensor<800){
// SysTick_Wait10ms(100);
// }
// else if (frontSensor<1000){
// SysTick_Wait10ms(50);
// }
SysTick_Wait10ms(158);
SetMotor(rightMotor, 0); //STOP
SetMotor(leftMotor, 0);
SetPin(PIN_F3, 1);
SetServo(servo,0);
SysTick_Wait10ms(50);
SetMotor(rightMotor, -.5);
SetMotor(leftMotor, .5);
while(true){
SetPin(PIN_F3, 1);
SetPin(PIN_F2, 0);
SetPin(PIN_F1, 0);
SysTick_Wait10ms(50);
SetPin(PIN_F3, 0);
SetPin(PIN_F2, 1);
SetPin(PIN_F1, 0);
SysTick_Wait10ms(50);
SetPin(PIN_F3, 0);
SetPin(PIN_F2, 0);
SetPin(PIN_F1, 1);
SysTick_Wait10ms(50);
}
}
