/* 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); } }