Subversion Repositories Projects

Rev

Rev 964 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 964 Rev 967
Line -... Line 1...
-
 
1
//////////////////////////////////////
-
 
2
// LocoHead - Mathias Kreider, 2011 //
-
 
3
//                                                                      //
-
 
4
// main.c                                                       //
-
 
5
// Main, PPM and Debug functions    //          
-
 
6
//////////////////////////////////////
-
 
7
1
#include <avr/io.h>   
8
#include <avr/io.h>   
2
#include <math.h>
9
#include <math.h>
3
#include <stdlib.h>
10
#include <stdlib.h>
4
#include "vector.h"
11
#include "vector.h"
5
#include <util/delay.h>
12
#include <util/delay.h>
Line 67... Line 74...
67
74
68
75
69
76
70
int main()
77
int main()
71
{
78
{
72
        _delay_ms(500);  //delay for VCC stabilization
79
        _delay_ms(500);  //delay for VCC stabilisation
73
       
80
       
74
        ppm_init();
81
        ppm_init();
75
       
82
       
76
        #if(DEBUG)
83
        #if(DEBUG)
77
                uart_init();
84
                uart_init();
Line 127... Line 134...
127
                       
134
                       
128
                        int thr_in, thr_out;           
135
                        int thr_in, thr_out;           
129
               
136
               
130
                        read_data(&a_avg, &m_avg);
137
                        read_data(&a_avg, &m_avg);
131
138
132
                        //get angle - startoffset
139
                        //calculate servo pulselength from sensor data
133
                        int heading_us  = thr_filter(get_us(get_heading(&a_avg, &m_avg, &p) - heading_start, HDG_MIN, HDG_MAX, HDG_US_MIN, HDG_US_MAX),&thr_in, &thr_out);
140
                        int heading_us  = thr_filter(get_us(get_heading(&a_avg, &m_avg, &p) - heading_start, HDG_MIN, HDG_MAX, HDG_US_MIN, HDG_US_MAX),&thr_in, &thr_out);
134
                        int pitch_us    = get_us(get_perpendicular(&a_avg, &down, &p) - pitch_start,            PIT_MIN, PIT_MAX, PIT_US_MIN, PIT_US_MAX);
141
                        int pitch_us    = get_us(get_perpendicular(&a_avg, &down, &p) - pitch_start,            PIT_MIN, PIT_MAX, PIT_US_MIN, PIT_US_MAX);
135
                        int roll_us     = get_us(get_perpendicular(&a_avg, &down, &right) - roll_start,         ROL_MIN, ROL_MAX, ROL_US_MIN, ROL_US_MAX);;    
142
                        int roll_us     = get_us(get_perpendicular(&a_avg, &down, &right) - roll_start,         ROL_MIN, ROL_MAX, ROL_US_MIN, ROL_US_MAX);;    
136
                       
143
                       
137
                        #if(DEBUG)
144
                        #if(DEBUG)