void FLOPPY_Init(void) { int i; for(i=0;i<FLOPPY_NOF_DRIVES;i++) { FLOPPY_Drives[i].forward = FALSE; FLOPPY_Drives[i].pos = 0; FLOPPY_Drives[i].currentPeriod = 0; FLOPPY_Drives[i].currentTick = 0; } FLOPPY_Drives[0].Dir = Drv0_Dir; FLOPPY_Drives[0].StepSetVal = Drv0_StepSetVal; FLOPPY_Drives[0].StepClearVal = Drv0_StepClrVal; FLOPPY_Drives[1].Dir = Drv1_Dir; FLOPPY_Drives[1].StepSetVal = Drv1_StepSetVal; FLOPPY_Drives[1].StepClearVal = Drv1_StepClrVal; FLOPPY_Drives[2].Dir = Drv2_Dir; FLOPPY_Drives[2].StepSetVal = Drv2_StepSetVal; FLOPPY_Drives[2].StepClearVal = Drv2_StepClrVal; FLOPPY_Drives[3].Dir = Drv3_Dir; FLOPPY_Drives[3].StepSetVal = Drv3_StepSetVal; FLOPPY_Drives[3].StepClearVal = Drv3_StepClrVal; FLOPPY_Drives[4].Dir = Drv4_Dir; FLOPPY_Drives[4].StepSetVal = Drv4_StepSetVal; FLOPPY_Drives[4].StepClearVal = Drv4_StepClrVal; FLOPPY_Drives[5].Dir = Drv5_Dir; FLOPPY_Drives[5].StepSetVal = Drv5_StepSetVal; FLOPPY_Drives[5].StepClearVal = Drv5_StepClrVal; FLOPPY_Drives[6].Dir = Drv6_Dir; FLOPPY_Drives[6].StepSetVal = Drv6_StepSetVal; FLOPPY_Drives[6].StepClearVal = Drv6_StepClrVal; FLOPPY_Drives[7].Dir = Drv7_Dir; FLOPPY_Drives[7].StepSetVal = Drv7_StepSetVal; FLOPPY_Drives[7].StepClearVal = Drv7_StepClrVal; TI1_Enable(); TI1_EnableEvent(); }
void main(void) { unsigned int position=0; int error=0,perror=0; long int iterm=0; //Declaration related to PID control systems. unsigned char com_data[7]; unsigned int servo_output=1500; int correction; unsigned char lap=0; unsigned int totalsum; int start_count=0; //variable to be removed... // DC pid control variables static int error_dc,perror_dc=0; //error can be +ve or -ve int correction_dc=0; //+ve or -ve unsigned char speed_dc = 40; unsigned char count=0; Leds_and_Switches_Init(); EEPROM_Init(); //EEPROM_reset(); EEPROM_read(); dc_motor_init(); // Current pwm duty is 85 ISR_init(); ATD_init(); pulse_counter_init(); servo_init(); //uart0_9600_init(); //To be removed in the final version of the software. TI1_Enable(); while(1) { //This function compensates the sen_data array and puts the result into com_data array. compensate_value(sen_data,com_data); // Placing it here, will increase the lag between value read and action taken.. hence // making the system unresponsive ... ATD0CTL5_SCAN=0; //This will start a new conversion .... // CODE FOR DC_PID CONTROL.. if(dc_newval_flag) { dc_newval_flag=0; perror_dc=error_dc; error_dc = setpoint - pulse_count; correction_dc =(int) (((KP_DC*error_dc )/DIV10)+ (KD_DC*(error_dc-perror_dc)/DIV10)); if(speed_dc + correction_dc>=200) { speed_dc=200; } else if(speed_dc + correction_dc <=0) { speed_dc =0; } else { // A type of integrator speed_dc +=correction_dc; } dc_motor_speed(speed_dc); } //START STOP DETEECTION //119 15 116 221 134 52 189 totalsum = com_data[4]+com_data[5]+com_data[6]+com_data[0]+com_data[1]+com_data[2]+com_data[3]; if(totalsum>START_OR_CROSS && totalsum<SURE_START) //either CROSS or start ... { //SUM OF SENSORS IS LARGE OS EITHER A CROSS OR START.. //LED1=LED_ON ; if(start_count>delay_dc) { if(lap>=2) { dc_motor_stop(); //TSCR1_TEN = 0; // Stop timer //dc_newval_flag=0; } else { lap++; } } } // CAN BE COMMENTED IN FINAL VERSION OF CODE else if(totalsum<ALL_WHITE) { //LED2=LED_ON ; //do nothing.. } else { //LED3=LED_ON ; start_count=0; //for start/cross detection sort_array(com_data); /*To find the correction, we have three branches .... */ //GUARD 1 if(index ==0 && com_data[index]<GUARD_VAL) { //LEFT //maore the 1500 correction =MAX_CORRECTION; //LED4=LED_ON; } //GUARD 2 else if(index==6 && com_data[index]<GUARD_VAL) { //RIGHT // less than 1500 correction =MIN_CORRECTION; //LED4=LED_ON; } else { //LED3=LED_OFF; position = (index << 9) + (com_data[index+1]-com_data[index-1]) ; perror=error; //stores previous error error = 1536 - position; //PID correction = (int)((int)((kp*error)/DIV100) + (int)((ki *iterm)/DIV1000) + (int)(kd*(error-perror)/DIV1000)); //anti wind up circutary if (correction >= MAX_CORRECTION) { correction = MAX_CORRECTION; } else if (correction <= MIN_CORRECTION) { correction = MIN_CORRECTION; } else { // limits bound on iterm. if (iterm>=ITERM_LIMIT) { //To buffer the fast responses of propotional control, we have the integral control. iterm = ITERM_LIMIT; //LED4=~LED4; } else if (iterm<=-ITERM_LIMIT) { //To buffer the fast responses of propotional control, we have the integral control. iterm = -ITERM_LIMIT; //LED4=~LED4; } else { iterm += error/20; } } } /* PID of dc motor should be applied based on the result of the pid of SERVO ie ""correction"" As the frequency of action of DC PID is slow,, 65 ms, so the lag is enough to drive the car out of track.. DIRECT ACCESS TO DC MOTOR SHOULD BE MADE .. while settiing thr """setpoint""" */ // correction will be handled from the guards also... if(correction > SHARP_TURN || (-correction) > SHARP_TURN) { //dc_motor_speed(10); setpoint=2; } else if ((correction>MIDDLE_TURN && correction <SHARP_TURN) || (correction < -MIDDLE_TURN && correction >-SHARP_TURN)) { //dc_motor_speed(20); setpoint=3; } else { //dc_motor_speed(50); setpoint =4; } servo_output = (unsigned int)(SET_POINT-correction); servo_set(servo_output); } } }