Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- /*
- * ATmega32L_ADC_test.c
- *
- * Created: 03.12.2011 18:53:45
- * Author: Sabesto
- */
- #define F_CPU 8000000UL
- #define USART_BAUDRATE 9600
- #define BAUD_PRESCALE (((F_CPU / (USART_BAUDRATE * 16UL))) - 1)
- #include <avr/io.h>
- #include <util/delay.h>
- #include <avr/interrupt.h>
- volatile uint8_t newSampleReady = 0;
- volatile int8_t channel = 0;
- volatile uint8_t ad[8];
- uint32_t teller = 0;
- uint32_t nevner = 0;
- uint8_t posisjon = 0;
- volatile uint64_t millis = 0;
- int8_t Setpoint = 50;
- int8_t Kp, Ki, Kd;
- volatile uint8_t calibration[8];
- int prev_res=0, prev_err_1=0, prev_err_2=0;
- uint8_t MaxSpeed = 0xFF;
- void timer1init(void){
- TCCR1B |= (1<<CS10);
- //TCCR1B |= (1<<CS11);
- //TCCR1B |= (1<<CS12);
- TIMSK |= (1<<TOIE1); // timer1 overflow interrupt enabled
- }
- void uartinit(void)
- {
- UCSRB |= (1 << RXEN) | (1 << TXEN);
- UCSRC |= (1 << UCSZ0) | (1 << UCSZ1);
- UBRRH = (BAUD_PRESCALE >> 8);
- UBRRL = BAUD_PRESCALE;
- }
- void adcinit(void){
- ADCSRA |= (1 << ADPS2) | (1 << ADPS1); // | (1 << ADPS0); // Set ADC prescalar to 64(128) - 125KHz sample rate @ 16MHz
- ADMUX = 0b01100000;
- //ADMUX |= (1 << REFS0); // Set ADC reference to AVCC
- //ADMUX |= (1 << ADLAR); // Left adjust ADC result to allow easy 8 bit reading
- ADCSRA |= (1 << ADATE); // Set ADC to Free-Running Mode
- ADCSRA |= (1 << ADEN); // Enable ADC
- ADCSRA |= (1 << ADIE); // enable adc interrupt
- ADCSRA |= (1 << ADSC); // Start A2D Conversions
- }
- void initall(void){
- DDRD |= (1<<PD7); // Toggle PIN for scope
- timer1init();
- uartinit();
- adcinit();
- sei();
- }
- void USART1_Transmit(unsigned char data)
- {
- while (!(UCSRA & (1<<UDRE)));
- UDR = data;
- }
- void USART1_Transmituint16(uint16_t data)
- {
- uint8_t low = data;
- uint8_t high = (data>>8);
- while (!(UCSRA & (1<<UDRE)));
- UDR = high;
- while (!(UCSRA & (1<<UDRE)));
- UDR = low;
- }
- void USART1_TransmitString (const char *str) {
- while (*str)
- {
- USART1_Transmit(*str);
- str++;
- }
- }
- uint8_t sensorvalue(void){
- teller=0;
- nevner=0;
- posisjon=0;
- for (uint16_t x=1;x<8;x++)
- {
- if(ad[x] < calibration[x])
- {ad[x]=0;}
- else
- {ad[x] -= calibration[x];}
- teller += ((x-1)<<4)*(ad[x]);
- nevner += ad[x];
- }
- //Value from the line sensor (0-96 where 0 is far left, 96 is far right)
- return (teller/nevner);
- }
- void debugled(uint8_t posisjon){
- if(posisjon < 14){PORTC = 0b00000001;}
- if(posisjon > 14 && posisjon < 28){PORTC = 0b00000010;}
- if(posisjon > 28 && posisjon < 42){PORTC = 0b00000100;}
- if(posisjon > 42 && posisjon < 56){PORTC = 0b00001000;}
- if(posisjon > 56 && posisjon < 70){PORTC = 0b00010000;}
- if(posisjon > 70 && posisjon < 82){PORTC = 0b00100000;}
- if(posisjon > 82 && posisjon < 100){PORTC = 0b01000000;}
- }
- void debugad(void){
- for (uint8_t x = 1;x<8;x++)
- {
- USART1_Transmit(ad[x]);
- _delay_ms(10);
- }
- }
- void calibrate(void){
- while (newSampleReady==0){}
- if (newSampleReady == 1)
- {
- for (uint8_t x = 0; x<8; x++)
- {
- calibration[x]=ad[x];
- }
- newSampleReady = 0;
- ADCSRA |= (1<<ADIE);
- }
- while (newSampleReady==0){}
- if (newSampleReady == 1)
- {
- for (uint8_t x = 0; x<8; x++)
- {
- calibration[x]=ad[x];
- }
- newSampleReady = 0;
- ADCSRA |= (1<<ADIE);
- }
- }
- void SetTunings(uint32_t kp, uint32_t ki, uint32_t kd)
- {
- kp = Kp;
- ki = Ki;
- kd = Kd;
- }
- /*
- void BRAM_Steer(unsigned char steer)
- {
- switch(steer) {
- case MOVE_FORWARD:
- PORTD &= ~(1 << PD4); PORTD |= (1 << PD7); // Right Motor On Forward
- PORTD &= ~(1 << PD2); PORTD |= (1 << PD3); // Left Motor On Forward
- break;
- case TURN_LEFT:
- PORTD &= ~(1 << PD4); PORTD |= (1 << PD7); // Right Motor On Forward
- PORTD &= ~(1 << PD2); PORTD &= ~(1 << PD3); // Left Motor Off
- break;
- case TURN_RIGHT:
- PORTD &= ~(1 << PD4); PORTD &= ~(1 << PD7); // Right Motor Off
- PORTD &= ~(1 << PD2); PORTD |= (1 << PD3); // Left Motor On Forward
- break;
- case ROTATE_LEFT:
- PORTD &= ~(1 << PD4); PORTD |= (1 << PD7); // Right Motor On Forward
- PORTD |= (1 << PD2); PORTD &= ~(1 << PD3); // Left Motor On Reverse
- break;
- case ROTATE_RIGHT:
- PORTD |= (1 << PD4); PORTD &= ~(1 << PD7); // Right Motor On Reverse
- PORTD &= ~(1 << PD2); PORTD |= (1 << PD3); // Left Motor On Forward
- break;
- case MOVE_BACKWARD:
- PORTD |= (1 << PD4); PORTD &= ~(1 << PD7); // Right Motor On Reverse
- PORTD |= (1 << PD2); PORTD &= ~(1 << PD3); // Left Motor On Reverse
- break;
- case FULL_STOP:
- PORTD &= ~(1 << PD4); PORTD &= ~(1 << PD7); // Right Motor Off
- PORTD &= ~(1 << PD2); PORTD &= ~(1 << PD3); // Left Motor Off
- break;
- }
- }
- void BRAM_DriveMotor(int left_speed, int right_speed)
- {
- unsigned char left_pwm,right_pwm;
- if (left_speed >= 0 && right_speed >= 0) {
- // Move Forward
- BRAM_Steer(MOVE_FORWARD);
- left_pwm=left_speed;
- right_pwm=right_speed;
- } else if (left_speed < 0 && right_speed < 0) {
- // Move Backward
- BRAM_Steer(MOVE_BACKWARD);
- left_pwm=left_speed * -1;
- right_pwm=right_speed * -1;
- } else if (left_speed < 0 && right_speed >= 0) {
- // Rotate Left
- BRAM_Steer(ROTATE_LEFT);
- left_pwm=left_speed * -1;
- right_pwm=right_speed;
- } else if (left_speed >= 0 && right_speed < 0) {
- // Rotate Right
- BRAM_Steer(ROTATE_RIGHT);
- left_pwm=left_speed;
- right_pwm=right_speed * -1;
- } else {
- // Full Stop
- BRAM_Steer(FULL_STOP);
- left_pwm=0;
- right_pwm=0;
- }
- // Assigned the value to the PWM Output Compare Registers A and B
- OCR0A=right_pwm;
- OCR0B=left_pwm;
- }
- */
- void pidcontrol(unsigned int sensor_val)
- {
- int motor_res,err_func;
- float KP,KI,KD,cont_res;
- // Get the Error Function
- err_func=sensor_val - Setpoint;
- // Divide all the PID parameters for decimal value
- KP=Kp * 0.1;
- KI=Ki * 0.01;
- KD=Kd * 0.01;
- // Calculate the Motor Response using PID Control Equation
- cont_res=(float)(prev_res + KP * (err_func - prev_err_1) + KI * (err_func + prev_err_1)/2.0
- + KD * (err_func - 2.0 * prev_err_1 + prev_err_2));
- // Now we have to Limit the control response to the Maximum of our motor PWM Motor Value
- motor_res=(int)cont_res;
- if (motor_res > MaxSpeed)
- motor_res = MaxSpeed;
- if (motor_res < -MaxSpeed)
- motor_res = -MaxSpeed;
- // Save the Motor Response and Error Function Result
- prev_res=motor_res;
- prev_err_2=prev_err_1;
- prev_err_1=err_func;
- /* Display Motor Response Value in Debugging Mode
- #if BRAM_DEBUG
- ansi_cm(4,1);
- printf("Motor Response Factor: %d ",motor_res);
- #endif
- */
- // Negative result mean BRAM is on the right, so we need to adjust to the left
- // Positive result mean BRAM is on the left, so we need to adjust to the right
- /*
- if (motor_res < 0)
- BRAM_DriveMotor(MaxSpeed + motor_res,MaxSpeed); // Left less speed, Right full speed
- else
- BRAM_DriveMotor(MaxSpeed,MaxSpeed - motor_res); // Left full speed, Right less speed
- */
- USART1_Transmituint16(motor_res);
- }
- int main (void)
- {
- _delay_ms(2000);
- SetTunings(10,0,0);
- DDRC = 0xff;
- initall();
- sei();
- calibrate();
- while(1)
- {
- if (newSampleReady == 1)
- {
- PORTD |= (1<<PD7); // connected to scope to measure calc time
- pidcontrol(sensorvalue())
- ADCSRA |= (1<<ADIE); // enable ADC interrupt
- PORTD &= ~(1<<PD7); // connected to scope to measure calc time
- newSampleReady = 0;
- }
- }
- }
- ISR(ADC_vect)
- {
- ad[channel] = ADCH; // Store the result (8-bits, discard the 3 LSB)
- // ADCSRA &= ~(1 << ADEN); // Disable the ADC
- channel ++;
- if (channel == 8)
- {
- ADCSRA &= ~(1<<ADIE);
- // disable interrupts for now
- newSampleReady = 1; // 7 channels sampled
- channel = 0; // Reset to channel 0
- }
- ADMUX &= 0xE0;//Update the ADMUX register
- ADMUX |= channel;
- }
- SIGNAL(TIMER1_OVF_vect)
- {
- TCNT1 = 0xF0FF;
- millis++;
- }
Add Comment
Please, Sign In to add comment