//************************************************************** // // Dateiname: Motorsteruerung.c // Datum: 04.02.2012 // Version: V1.0 // // Autor: Philip Broser // Internet: // e-mail: Philip.Broser@me.com // //************************************************************** #pragma config FOSC = INTIO67, FCMEN = OFF, IESO = OFF // CONFIG1H #pragma config PWRT = OFF, BOREN = OFF, BORV = 30 // CONFIG2L #pragma config WDTEN = OFF, WDTPS = 32768 // CONFIG2H #pragma config MCLRE = ON, LPT1OSC = OFF, PBADEN = OFF, CCP2MX = PORTC // CONFIG3H #pragma config STVREN = ON, LVP = OFF, XINST = OFF // CONFIG4L #pragma config CP0 = OFF, CP1 = OFF // CONFIG5L #pragma config CPB = OFF, CPD = OFF // CONFIG5H #pragma config WRT0 = OFF, WRT1 = OFF // CONFIG6L #pragma config WRTB = OFF, WRTC = OFF, WRTD = OFF // CONFIG6H #pragma config EBTR0 = OFF, EBTR1 = OFF // CONFIG7L #pragma config EBTRB = OFF // CONFIG7H /** I N C L U D E S *******************************************/ #include #include "delays.h" #include /** D E F I N E S *********************************************/ #define TASTER1 PORTBbits.RB0 //Taster liegt an Port B RB0 #define TASTER2 PORTBbits.RB1 //Taster liegt an Port B RB0 #define ServoSensor PORTBbits.RB2 #define DistanceSensor PORTBbits.RB3 // Distance Sensor #define LED LATAbits.LATA0 //LED liegt an Port A RA4 void low_isr (void); void low_isr(void); void high_isr(void); #pragma code high_vector=0x08 void interrupt_at_high_vector(void) { _asm GOTO high_isr _endasm } #pragma code unsigned char servo_state=0; unsigned char servo_pos=0; #pragma interrupt high_isr void high_isr (void) { if (servo_state==0) { ServoSensor=0; TMR0H=251; //wait approx 10ms TMR0L=50; servo_state=1; } else { ServoSensor=1; TMR0H=255; TMR0L=255-(125+servo_pos); // 1ms + position value 0..125 TMR0L=servo_pos; // 1ms + position value 0..125 servo_state=0; }; INTCONbits.TMR0IF = 0; } #pragma code low_vector=0x18 void low_interrupt (void) { _asm GOTO low_isr _endasm } #pragma code #pragma interruptlow low_isr void low_isr (void) { } #define FULL_POWER 200 /** H A U P T P R O G R A M M *********************************/ void main(void) { int engine=0; int direction=0; int mode=0; int obstacle=0; //Setup Port Pins for external connection //Port A LATA = 0x00; TRISA = 0x00; //Alle Pins von Port A sind Ausgänge LATB = 0x00; //TRISB = 0xFF; //Alle Pins von Port B sind Eingänge //RB 0,1 Taster //RB 2 Servo //RB 3 Distance Sensor TRISB = 0b11111011; // RB2 steuert servo PORTBbits.AN12=0; PORTBbits.AN10=0; PORTBbits.AN8=0; PORTBbits.AN9=0; ANSELHbits.ANS12=0; ANSELHbits.ANS10=0; ANSELHbits.ANS9=0; ANSELHbits.ANS8=0; //Port C LATC = 0x00; TRISC = 0xF0; //Init T0 for Servo T0CON = 0b10000000;// on,16bit,1:2 prescale // 1MHZ / 4 = 250 kHZ Instruction / 2 = 125 000 counts per second // 1 ms are 125 //10ms are 1250 TMR0H = 0; TMR0L = 0; //Init PWM for Engine Controll // Set up 8-bit Timer2 to generate the PWM period (frequency) T2CON = 0b00000111;// Prescale = 1:16, timer on, postscale not used with CCP module PR2 = 249; // Timer 2 Period Register = 250 counts // Thus, the PWM frequency is: // 1 MHz clock / 4 = 250 kHz instruction rate. // ( 250kHz / 16 prescale) / 250) = kHz, a period of 0ms. //Set UP PWM 1 Module // The Duty Cycle is controlled by the ten-bit CCPR1L<7,0>:DC1B1:DC1B0 // 50% Duty Cycle = 0.5 * (250 * 4) = 500 CCPR1L = 0x7D; // The 8 most significant bits of the value 500 = 0x1F4 are 0x7D // The 2 least significant bits of the value (0b00) are written // to the DC1B1 and DC1B0 bits in CCP1CON CCP1CON = 0b00001100; CCP1CONbits.DC1B0=1; CCP1CONbits.DC1B1=1; //PWM Power Control CCPR1L=0; // The Duty Cycle is controlled by the ten-bit CCPR1L<7,0>:DC1B1:DC1B0 // 50% Duty Cycle = 0.5 * (250 * 4) = 500 CCPR2L = 0x7D; // The 8 most significant bits of the value 500 = 0x1F4 are 0x7D // The 2 least significant bits of the value (0b00) are written // to the DC1B1 and DC1B0 bits in CCP1CON CCP2CON = 0b00001100; CCP2CONbits.DC2B0=1; CCP2CONbits.DC2B1=1; //PWM Power Control CCPR2L=0; //Init Timer1 for Cruise Controll T1CONbits.TMR1CS=0; //T1CONbits.TMR1CS0=0; T1CON=0b00110101; //FOSC/4, 1:8 Prescaler, TMR1H=255; TMR1L=150; PIR1bits.TMR1IF=0; //Switch on Interrupt INTCONbits.T0IF=0; INTCONbits.T0IE=1; RCONbits.IPEN=0; INTCON2bits.TMR0IP=0; servo_pos=115; //Middle Position INTCONbits.GIE = 1; LATAbits.LATA1=0; LATAbits.LATA2=1; LATAbits.LATA3=0; LATAbits.LATA4=1; //Remark //Left Wheels CCPR2L //Right Wheels CCPR1L #define POWER_LEFT CCPR1L #define POWER_RIGHT CCPR2L //Right Forward //LATAbits.LATA1=0; //LATAbits.LATA2=1; #define Set_Right_Forward {LATAbits.LATA1=0;LATAbits.LATA2=1;} #define Set_Right_Backward {LATAbits.LATA1=1;LATAbits.LATA2=0;} #define Set_Right_Off {LATAbits.LATA1=0;LATAbits.LATA2=0;} //Left Forward //LATAbits.LATA3=0; //LATAbits.LATA4=1; #define Set_Left_Forward {LATAbits.LATA3=0;LATAbits.LATA4=1;} #define Set_Left_Backward {LATAbits.LATA3=1;LATAbits.LATA4=0;} #define Set_Left_Off {LATAbits.LATA3=0;LATAbits.LATA4=0;} #define MOTOR_AN {CCPR1L=200;CCPR2L=200;} #define MOTOR_AUS {CCPR1L=0;CCPR2L=0;} //servo_pos //links 10 middle 115 right 230 //#define SERVO_LEFT_LOW 10 //#define SERVO_RIGHT_HIGH 230 //#define SERVO_MIDDLE 115 //#define SERVO_DELTA_FRONTAL 50 //links 10 middle 115 right 230 #define SERVO_LEFT_LOW 10 #define SERVO_RIGHT_HIGH 230 #define SERVO_MIDDLE 115 #define SERVO_DELTA_FRONTAL 30 #define LONG_SERVO_WAIT {PIR1bits.TMR1IF=0;TMR1H=140;TMR1L=0;while (PIR1bits.TMR1IF==0) {};}; while(0) { PIR1bits.TMR1IF=0; TMR1H=140; TMR1L=0; while (PIR1bits.TMR1IF==0) { //Wait for car to stop and sensor to turn left }; LED=!LED; if (LED) {servo_pos=30;} else servo_pos=210; }; //Test Servo INterfernce while(0) { servo_pos=SERVO_LEFT_LOW; {PIR1bits.TMR1IF=0;TMR1H=140;TMR1L=0;while (PIR1bits.TMR1IF==0) { if (DistanceSensor ) {LED=1;} else LED=0;};}; for (servo_pos=SERVO_LEFT_LOW;servo_pos(SERVO_MIDDLE+SERVO_DELTA_FRONTAL);servo_pos=servo_pos-1) { TMR1H=254; TMR1L=100; //only small steps PIR1bits.TMR1IF=0; while (PIR1bits.TMR1IF==0) { //Wait for servo to adjust and sensor to be ready }; if (DistanceSensor ) {LED=1;} else LED=0; }; }; //Test servo and dist sensor while(0) { Delay10TCYx(100); if (!TASTER1) servo_pos=servo_pos+1; if (!TASTER2) servo_pos=servo_pos-1; if (servo_pos<2) servo_pos=1; if (servo_pos>230) servo_pos=230; if (DistanceSensor) { LED=1; } else { LED=0; }; }; while(1) { //if (!TASTER1) engine=1; if (!TASTER2) { if (engine) {engine=0;} else engine=1; }; //Servo View Direction Controll if (PIR1bits.TMR1IF) { PIR1bits.TMR1IF=0; TMR1H=255; TMR1L=150; if (direction) { servo_pos=servo_pos+1; if (servo_pos>(SERVO_MIDDLE+SERVO_DELTA_FRONTAL)) direction=0; } else { servo_pos=servo_pos-1; if (servo_pos<(SERVO_MIDDLE-SERVO_DELTA_FRONTAL)) direction=1; }; }; if (DistanceSensor ) {LED=1;} else LED=0; if (DistanceSensor & engine ) { int obstacle = 1; while (obstacle) { int direction_counter=0; int obstacle_left=0; int obstacle_right=0; POWER_LEFT=0;POWER_RIGHT=0; // Stop Engines servo_pos=SERVO_LEFT_LOW; //Wait for car to stop and sensor to turn left LONG_SERVO_WAIT; LONG_SERVO_WAIT; //Look Arround and locate objects //Look Left for (servo_pos=SERVO_LEFT_LOW;servo_pos(SERVO_MIDDLE+SERVO_DELTA_FRONTAL);servo_pos=servo_pos-1) { TMR1H=254; TMR1L=100; //only small steps PIR1bits.TMR1IF=0; while (PIR1bits.TMR1IF==0) { //Wait for servo to adjust and sensor to be ready }; if (DistanceSensor) obstacle_right=1; if (DistanceSensor ) {LED=1;} else LED=0; }; servo_pos=SERVO_MIDDLE; LONG_SERVO_WAIT; //No Obstacle right so turn right if (obstacle_right==0 & obstacle) { //Turn right Set_Left_Forward; Set_Right_Backward; POWER_LEFT=FULL_POWER; POWER_RIGHT=FULL_POWER; TMR1H=150;PIR1bits.TMR1IF=0; TMR1L=200; //only small steps while (PIR1bits.TMR1IF==0) { //Wait for car toturn }; POWER_LEFT=0;POWER_RIGHT=0; Set_Left_Forward; Set_Right_Forward; obstacle=0; }; //No Obstacle left so turn left if (obstacle_left==0 & obstacle) { Set_Left_Backward; Set_Right_Forward; POWER_LEFT=FULL_POWER;POWER_RIGHT=FULL_POWER; TMR1H=150;PIR1bits.TMR1IF=0; TMR1L=200; //only small steps while (PIR1bits.TMR1IF==0) { //Wait for car toturn }; Set_Left_Forward; Set_Right_Forward; POWER_LEFT=0;POWER_RIGHT=0; obstacle=0; }; //obstacle left and right so go backwards if (obstacle ) { Set_Left_Backward; Set_Right_Backward; POWER_LEFT=FULL_POWER;POWER_RIGHT=FULL_POWER; TMR1H=200;PIR1bits.TMR1IF=0; TMR1L=200; //only small steps while (PIR1bits.TMR1IF==0) { //Wait for car toturn }; POWER_LEFT=0;POWER_RIGHT=0; Set_Left_Forward; Set_Right_Forward; }; }; }; if (engine) {POWER_LEFT=FULL_POWER;POWER_RIGHT=FULL_POWER;} else {CCPR1L=0;CCPR2L=0;} Set_Left_Forward; Set_Right_Forward; if (DistanceSensor) LED=1; }; }