/* SetMode(...)****************************************************************
* Allows the controller Mode to be set to manual (0) or Automatic (non-zero)
* when the transition from manual to auto occurs, the controller is
* automatically initialized
******************************************************************************/
void PID_SetMode(PidType* pid, PidModeType Mode)
{
bool newAuto = (Mode == PID_Mode_Automatic);
if(newAuto == !pid->inAuto)
{ /*we just went from manual to auto*/
PID_Initialize(pid);
}
pid->inAuto = newAuto;
}
/**Program Entry Point*/
int main()
{
JOINT joints[2];
STATE_FLAGS flags;
OBJECT_LOCATION objLoc;
DATA_PACK dataPack;
char state = 0;
char input;
//initialize pointers in mlMessage
dataPack.flags = &flags;
dataPack.joint = &joints[0];
dataPack.location = &objLoc;
//Set initial values for the state flags
flags._FLAGS = 0;
//Set initial values for the joints
joints[0].linkLength = 200;
joints[0].targetVal = 0;
joints[0].currentVal = 0;
joints[0].LMDVal = 0;
joints[0].correctionVal = 0;
joints[1].linkLength = 195;
joints[1].targetVal = 0;
joints[1].currentVal = 0;
joints[1].LMDVal = 0;
joints[1].correctionVal = 0;
//Set direction registers for debugging use
DDRD = 0xFF;
DDRC = 0x00;
//Initialize serial communications, the ADC, the DAC and PID control
Init_Serial_P(BAUD576, FRM8, STOP1, NOPAR);
ADC_Init_P();
DAC_Initilize();
PID_Initialize();
Serial_Print_String("\r\nMag Pickup Test\r\n");
/*while(1)
{
Serial_Print_String("\r\nIR_X: ");
Serial_Print_Int(ReadIR(IR_X), 10);
Serial_Print_String("\r\nIR_Y: ");
Serial_Print_Int(ReadIR(IR_Y), 10);
_delay_ms(200);
}*/
//Calibrate joint pots
Add_Pot_Angle(542, 90, 175, 0, "Joint 1", JOINT1);
Add_Pot_Angle(504, 90, 849, 0, "Joint 2", JOINT2);
//Add PID channels to control the two joints
PID_Add_Channel(&joints[0].targetVal, &joints[0].currentVal, &joints[0].correctionVal, &joints[0].LMDVal, 450, 50, 300, 250);
PID_Add_Channel(&joints[1].targetVal, &joints[1].currentVal, &joints[1].correctionVal, &joints[1].LMDVal, 375, 10, 350, 150);
while(1)
{
joints[0].currentVal = ADC_Get_Value(JOINT1);
joints[1].currentVal = ADC_Get_Value(JOINT2);
joints[0].targetVal = Degrees_To_Pot(30, JOINT1);
joints[1].targetVal = Degrees_To_Pot(60, JOINT2);
State_Machine(&state, &dataPack);
/*Serial_Print_String("\r\nTarget: ");
Serial_Print_Int(Pot_To_Degrees(joints[1].targetVal, JOINT2), 10);
Serial_Print_String("\r\nCurrent: ");
Serial_Print_Int(Pot_To_Degrees(joints[1].currentVal, JOINT2), 10);*/
_delay_ms(100);
}
}
