Guest User

Untitled

a guest
Apr 21st, 2018
67
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C 8.04 KB | None | 0 0
  1. /*
  2.  * ATmega32L_ADC_test.c
  3.  *
  4.  * Created: 03.12.2011 18:53:45
  5.  *  Author: Sabesto
  6.  */
  7.  
  8. #define F_CPU 8000000UL
  9. #define USART_BAUDRATE 9600
  10. #define BAUD_PRESCALE (((F_CPU / (USART_BAUDRATE * 16UL))) - 1)
  11. #include <avr/io.h>
  12. #include <util/delay.h>
  13. #include <avr/interrupt.h>
  14.  
  15. volatile uint8_t newSampleReady = 0;
  16. volatile int8_t channel = 0;
  17. volatile uint8_t ad[8];
  18. uint32_t teller = 0;
  19. uint32_t nevner = 0;
  20. uint8_t posisjon = 0;
  21. volatile uint64_t millis = 0;
  22. int8_t Setpoint = 50;
  23. int8_t Kp, Ki, Kd;
  24. volatile uint8_t calibration[8];
  25. int prev_res=0, prev_err_1=0, prev_err_2=0;
  26. uint8_t MaxSpeed = 0xFF;
  27.  
  28. void timer1init(void){
  29.     TCCR1B |= (1<<CS10);
  30.     //TCCR1B |= (1<<CS11);
  31.     //TCCR1B |= (1<<CS12); 
  32.     TIMSK |= (1<<TOIE1);    // timer1 overflow interrupt enabled
  33. }
  34. void uartinit(void)
  35. {
  36.     UCSRB |= (1 << RXEN) | (1 << TXEN);  
  37.     UCSRC |= (1 << UCSZ0) | (1 << UCSZ1);
  38.     UBRRH = (BAUD_PRESCALE >> 8);
  39.     UBRRL = BAUD_PRESCALE;
  40. }
  41. void adcinit(void){
  42.     ADCSRA |= (1 << ADPS2) | (1 << ADPS1); // | (1 << ADPS0); // Set ADC prescalar to 64(128) - 125KHz sample rate @ 16MHz
  43.     ADMUX = 0b01100000;
  44.     //ADMUX |= (1 << REFS0); // Set ADC reference to AVCC
  45.     //ADMUX |= (1 << ADLAR); // Left adjust ADC result to allow easy 8 bit reading
  46.     ADCSRA |= (1 << ADATE);  // Set ADC to Free-Running Mode
  47.     ADCSRA |= (1 << ADEN);  // Enable ADC
  48.     ADCSRA |= (1 << ADIE);  // enable adc interrupt
  49.     ADCSRA |= (1 << ADSC);  // Start A2D Conversions
  50.    
  51. }
  52. void initall(void){
  53.     DDRD |= (1<<PD7); // Toggle PIN for scope
  54.     timer1init();
  55.     uartinit();
  56.     adcinit();
  57.     sei();
  58. }
  59.  
  60. void USART1_Transmit(unsigned char data)
  61. {
  62.    while (!(UCSRA & (1<<UDRE)));                        
  63.    UDR = data;                
  64. }
  65. void USART1_Transmituint16(uint16_t data)
  66. {
  67.     uint8_t low = data;
  68.     uint8_t high = (data>>8);
  69.    
  70.    while (!(UCSRA & (1<<UDRE)));                        
  71.    UDR = high;
  72.    while (!(UCSRA & (1<<UDRE)));                        
  73.    UDR = low;                  
  74. }
  75. void USART1_TransmitString (const char *str) {
  76.  
  77.     while (*str)
  78.     {
  79.         USART1_Transmit(*str);
  80.         str++;
  81.     }    
  82. }
  83.  
  84. uint8_t sensorvalue(void){
  85.     teller=0;
  86.     nevner=0;
  87.     posisjon=0;
  88.     for (uint16_t x=1;x<8;x++)
  89.         {
  90.             if(ad[x] < calibration[x])
  91.                 {ad[x]=0;}
  92.             else
  93.                 {ad[x] -= calibration[x];}
  94.                        
  95.             teller += ((x-1)<<4)*(ad[x]);  
  96.             nevner += ad[x];
  97.         }
  98.         //Value from the line sensor (0-96 where 0 is far left, 96 is far right)
  99.     return (teller/nevner);
  100. }
  101.  
  102. void debugled(uint8_t posisjon){
  103.     if(posisjon < 14){PORTC = 0b00000001;}
  104.     if(posisjon > 14 && posisjon < 28){PORTC =  0b00000010;}
  105.     if(posisjon > 28 && posisjon < 42){PORTC =  0b00000100;}
  106.     if(posisjon > 42 && posisjon < 56){PORTC =  0b00001000;}
  107.     if(posisjon > 56 && posisjon < 70){PORTC =  0b00010000;}
  108.     if(posisjon > 70 && posisjon < 82){PORTC =  0b00100000;}
  109.     if(posisjon > 82 && posisjon < 100){PORTC = 0b01000000;}
  110. }
  111. void debugad(void){
  112.     for (uint8_t x = 1;x<8;x++)
  113.         {
  114.             USART1_Transmit(ad[x]);
  115.             _delay_ms(10);
  116.         }
  117. }
  118.  
  119. void calibrate(void){
  120.     while (newSampleReady==0){}
  121.     if (newSampleReady == 1)
  122.         {
  123.             for (uint8_t x = 0; x<8; x++)
  124.                 {
  125.                 calibration[x]=ad[x];
  126.                 }
  127.             newSampleReady = 0;
  128.             ADCSRA |= (1<<ADIE);
  129.         }
  130.     while (newSampleReady==0){}
  131.     if (newSampleReady == 1)
  132.         {
  133.             for (uint8_t x = 0; x<8; x++)
  134.                 {
  135.                 calibration[x]=ad[x];
  136.                 }
  137.             newSampleReady = 0;
  138.             ADCSRA |= (1<<ADIE);
  139.     }
  140. }
  141.  
  142. void SetTunings(uint32_t kp, uint32_t ki, uint32_t kd)
  143. {
  144.    kp = Kp;
  145.    ki = Ki;
  146.    kd = Kd;
  147. }
  148.  
  149. /*
  150. void BRAM_Steer(unsigned char steer)
  151. {
  152.   switch(steer) {
  153.     case MOVE_FORWARD:
  154.       PORTD &= ~(1 << PD4); PORTD |= (1 << PD7);  // Right Motor On Forward
  155.       PORTD &= ~(1 << PD2); PORTD |= (1 << PD3);  // Left Motor On Forward
  156.       break;
  157.     case TURN_LEFT:
  158.       PORTD &= ~(1 << PD4); PORTD |= (1 << PD7);  // Right Motor On Forward
  159.       PORTD &= ~(1 << PD2); PORTD &= ~(1 << PD3); // Left Motor Off
  160.       break;
  161.     case TURN_RIGHT:
  162.       PORTD &= ~(1 << PD4); PORTD &= ~(1 << PD7); // Right Motor Off
  163.       PORTD &= ~(1 << PD2); PORTD |= (1 << PD3);  // Left Motor On Forward
  164.       break;
  165.     case ROTATE_LEFT:
  166.       PORTD &= ~(1 << PD4); PORTD |= (1 << PD7);  // Right Motor On Forward
  167.       PORTD |= (1 << PD2); PORTD &= ~(1 << PD3);  // Left Motor On Reverse
  168.           break;
  169.     case ROTATE_RIGHT:
  170.       PORTD |= (1 << PD4); PORTD &= ~(1 << PD7);  // Right Motor On Reverse
  171.       PORTD &= ~(1 << PD2); PORTD |= (1 << PD3);  // Left Motor On Forward
  172.           break;
  173.     case MOVE_BACKWARD:
  174.       PORTD |= (1 << PD4); PORTD &= ~(1 << PD7);  // Right Motor On Reverse
  175.       PORTD |= (1 << PD2); PORTD &= ~(1 << PD3);  // Left Motor On Reverse
  176.           break;
  177.     case FULL_STOP:
  178.       PORTD &= ~(1 << PD4); PORTD &= ~(1 << PD7);  // Right Motor Off
  179.       PORTD &= ~(1 << PD2); PORTD &= ~(1 << PD3);  // Left Motor Off
  180.           break;
  181.   }
  182. }  
  183. void BRAM_DriveMotor(int left_speed, int right_speed)
  184. {
  185.   unsigned char left_pwm,right_pwm;
  186.  
  187.   if (left_speed >= 0 && right_speed >= 0) {
  188.     // Move Forward
  189.     BRAM_Steer(MOVE_FORWARD);
  190.  
  191.     left_pwm=left_speed;
  192.     right_pwm=right_speed;
  193.  
  194.   } else if (left_speed < 0 && right_speed < 0) {
  195.     // Move Backward
  196.     BRAM_Steer(MOVE_BACKWARD);
  197.  
  198.     left_pwm=left_speed * -1;
  199.     right_pwm=right_speed * -1;
  200.  
  201.   } else if (left_speed < 0 && right_speed >= 0) {
  202.     // Rotate Left
  203.     BRAM_Steer(ROTATE_LEFT);
  204.  
  205.     left_pwm=left_speed * -1;
  206.     right_pwm=right_speed;
  207.   } else if (left_speed >= 0 && right_speed < 0) {
  208.     // Rotate Right
  209.     BRAM_Steer(ROTATE_RIGHT);
  210.  
  211.     left_pwm=left_speed;
  212.     right_pwm=right_speed * -1;
  213.   } else {
  214.     // Full Stop
  215.     BRAM_Steer(FULL_STOP);
  216.  
  217.     left_pwm=0;
  218.     right_pwm=0;
  219.   }
  220.  
  221.   // Assigned the value to the PWM Output Compare Registers A and B
  222.   OCR0A=right_pwm;
  223.   OCR0B=left_pwm;
  224. }
  225. */
  226. void pidcontrol(unsigned int sensor_val)
  227. {
  228.     int motor_res,err_func;
  229.     float KP,KI,KD,cont_res;
  230.    
  231.     // Get the Error Function
  232.     err_func=sensor_val - Setpoint;
  233.  
  234.     // Divide all the PID parameters for decimal value
  235.     KP=Kp * 0.1;
  236.     KI=Ki * 0.01;
  237.     KD=Kd * 0.01;
  238.  
  239.     // Calculate the Motor Response using PID Control Equation
  240.     cont_res=(float)(prev_res + KP * (err_func - prev_err_1) + KI * (err_func + prev_err_1)/2.0
  241.                + KD * (err_func - 2.0 * prev_err_1 + prev_err_2));        
  242.  
  243.    // Now we have to Limit the control response to the Maximum of our motor PWM Motor Value
  244.    motor_res=(int)cont_res;
  245.  
  246.    if (motor_res > MaxSpeed)
  247.      motor_res = MaxSpeed;
  248.    if (motor_res < -MaxSpeed)
  249.      motor_res = -MaxSpeed;  
  250.  
  251.    // Save the Motor Response and Error Function Result
  252.    prev_res=motor_res;
  253.    prev_err_2=prev_err_1;
  254.    prev_err_1=err_func;
  255.  
  256.    /* Display Motor Response Value in Debugging Mode
  257.     #if BRAM_DEBUG
  258.    ansi_cm(4,1);
  259.    printf("Motor Response Factor: %d   ",motor_res);
  260.     #endif
  261.     */
  262.  
  263.    // Negative result mean BRAM is on the right, so we need to adjust to the left
  264.    // Positive result mean BRAM is on the left, so we need to adjust to the right
  265.    /*
  266.    if (motor_res < 0)
  267.      BRAM_DriveMotor(MaxSpeed + motor_res,MaxSpeed); // Left less speed, Right full speed
  268.    else
  269.      BRAM_DriveMotor(MaxSpeed,MaxSpeed - motor_res); // Left full speed, Right less speed
  270.      */
  271.    USART1_Transmituint16(motor_res);
  272. }
  273.  
  274. int main (void)
  275. {
  276.     _delay_ms(2000);
  277.     SetTunings(10,0,0);
  278.     DDRC = 0xff;
  279.    
  280.    
  281.     initall();
  282.     sei();
  283.     calibrate();           
  284.     while(1)
  285.     {
  286.         if (newSampleReady == 1)
  287.         {
  288.             PORTD |= (1<<PD7); // connected to scope to measure calc time
  289.             pidcontrol(sensorvalue())      
  290.             ADCSRA |= (1<<ADIE); // enable ADC interrupt
  291.             PORTD &= ~(1<<PD7); // connected to scope to measure calc time
  292.             newSampleReady = 0;
  293.         }
  294.        
  295.     }
  296. }
  297. ISR(ADC_vect)
  298. {
  299.     ad[channel] = ADCH; // Store the result (8-bits, discard the 3 LSB)
  300.     // ADCSRA &= ~(1 << ADEN); // Disable the ADC
  301.     channel ++;
  302.    
  303.        
  304.     if (channel == 8)
  305.     {
  306.         ADCSRA &= ~(1<<ADIE);
  307.         // disable interrupts for now
  308.         newSampleReady = 1; // 7 channels sampled
  309.         channel = 0; // Reset to channel 0
  310.     }  
  311.     ADMUX &= 0xE0;//Update the ADMUX register
  312.     ADMUX |= channel;
  313. }
  314. SIGNAL(TIMER1_OVF_vect)
  315. {
  316.    TCNT1 = 0xF0FF;
  317.    millis++;
  318. }
Add Comment
Please, Sign In to add comment