Project

General

Profile

Revision eb7af729

IDeb7af72914835eb125f11f3cc03af177370ba451
Parent e6c4b5e0
Child 5ea63f0d

Added by unknown almost 10 years ago

Arduino code measures PWM up and PWM down time, and if U+D!= contant, we reject the signal

View differences:

arduino/RadioBuggyMega/RadioBuggyMega.ino
3 3
 * @author Haley Dalzell (haylee)
4 4
 * @author Zach Dawson (zachyzach)
5 5
 * @author Matt Sebek (msebek)
6
  *@author 
6 7
 */
7 8
#include "receiver.h"
8 9
#include "brake.h"
......
93 94
static int steer_angle;
94 95

  
95 96
void loop() {
96

  
97 97
  //if(filter_loop(&thr_state, rc_available[THR_INDEX])) {
98
  if(rc_available[THR_INDEX]) {
98
  if(rc_available[AIL_INDEX]) {
99 99
    watchdog_feed();
100
    raw_angle = receiver_get_angle(THR_INDEX);
101
    smoothed_angle = filter_loop(&thr_state, raw_angle);
102
    steer_angle = convert_rc_to_steering(smoothed_angle);
100
    raw_angle = receiver_get_angle(AIL_INDEX);
101
    smoothed_angle = convert_rc_to_steering(raw_angle);
102
    steer_angle = filter_loop(&ail_state, smoothed_angle);
103 103
    steering_set(steer_angle);
104 104
  }
105 105

  
106 106
  //if(filter_loop(&ail_state, rc_available[AIL_INDEX])) {
107
  if(rc_available[AIL_INDEX]) {
107
  if(rc_available[THR_INDEX]) {
108 108
    watchdog_feed();
109
    raw_thr = receiver_get_angle(AIL_INDEX);
110
    smoothed_thr = filter_loop(&ail_state, raw_thr);
109
    raw_thr = receiver_get_angle(THR_INDEX);
110
    smoothed_thr = filter_loop(&thr_state, raw_thr);
111 111
    // TODO make this code...less...something
112 112
    if(smoothed_thr < 90) {
113 113
      brake_drop();
arduino/RadioBuggyMega/filter.c
8 8
    state->started = 0;
9 9
}
10 10

  
11
//<<<<<<< Updated upstream
11 12
int filter_loop(struct filter_state *state, int val){
12 13
     int i;
13 14
    // First initialization: on assumption val is never -1
14 15
    if(!(state->started)){
15 16
        for(i = 0; i < FILTER_LEN; i++){
16 17
            state->prev_values[i] = val;
18
//=======
19
//#define SIZE 5 // the array size
20
//static char started = 0;
21
//static int last_val_sent;
22
//static int values[SIZE];
23
//static int pos;
24
//
25
///**
26
// * @brief Filters noise from raw RC readings
27
// *
28
// */
29
//int filter(int new_raw_reading){
30
//     int i;
31
//   
32
//    // On Initialization, assume our "first" reading
33
//    //    occurred N times, and is our history
34
//    if(!started){
35
//        for(i = 0; i < SIZE; i++){
36
//            values[i] = new_raw_reading;
37
//>>>>>>> Stashed changes
17 38
        }
18 39
        state->pos = 0; // initialize the positions of the next number to be changed
19 40
        state->started = 1;
20 41
        return val;
21
    }
42
    } 
43
    
22 44
    
45
//<<<<<<< Updated upstream
23 46
    // NOT first initialization. 
24 47
    state->prev_values[state->pos] = val;  // replace the number in the array
25 48
    state->pos = (state->pos+1)%FILTER_LEN;// increment the pos number
26 49
    // calculate average of array
50
//=======
51
//    
52
//        
53
//    values[pos] = val;  // replace the number in the array
54
//    pos = (pos+1)%SIZE; // increment the pos number
55
//    
56
//    // calculate average of previous readings
57
//>>>>>>> Stashed changes
27 58
    int sum = 0;
28 59
    for(i = 0; i < FILTER_LEN; i++){
29 60
        sum += state->prev_values[i];
30 61
    }
62
//<<<<<<< Updated upstream
31 63
    int avg = sum/FILTER_LEN;
32 64
    // return.
33 65
    return avg;
66
//=======
67
//    // Return the average.
68
//    return sum / SIZE;
69
//>>>>>>> Stashed changes
34 70

  
35 71
}
arduino/RadioBuggyMega/filter.h
13 13
extern "C"{
14 14
#endif
15 15

  
16
#define FILTER_LEN 5 // Averaging window size
16
//<<<<<<< Updated upstream
17
#define FILTER_LEN 10 // Averaging window size
17 18

  
18 19
struct filter_state {
19 20
    int prev_values[FILTER_LEN];
......
25 26
void filter_init(struct filter_state *state);
26 27

  
27 28
int filter_loop(struct filter_state *state, int val); // returns the int that has been filtered
29
//=======
30
//// @param val The next raw reading
31
//// @returns The next filtered reading
32
//int filter(int new_raw_reading); 
33
//          
34
//>>>>>>> Stashed changes
28 35

  
29 36

  
30 37
#ifdef __cplusplus
arduino/RadioBuggyMega/receiver.c
20 20
#define THR_RIGHTMOST -1
21 21
#define THR_CENTERMOST -1
22 22

  
23
#define AIL_RECEIVER_PIN 3
24
#define AIL_RECEIVER_INT 1
23
#define AIL_RECEIVER_PIN 2
24
#define AIL_RECEIVER_INT 0
25 25

  
26
#define THR_RECEIVER_PIN 2
27
#define THR_RECEIVER_INT 0
26
#define THR_RECEIVER_PIN 3
27
#define THR_RECEIVER_INT 1
28

  
29
#define PWM_TIME 21800
30
#define PWM_THRESH 130
28 31

  
29 32
// Defined in receiver.h
30 33
// #define THR_INDEX 0
31 34
// #define AIL_INDEX 1
32 35

  
33 36
// Note: arr[0] is thr, arr[1] is ail
34
static volatile unsigned long start_time[2];
35
static volatile unsigned long rc_value[2];
36 37

  
38
static volatile unsigned long up_switch_time[2];
39
static volatile unsigned long down_switch_time[2];
40
static volatile unsigned long rc_value[2];
37 41

  
42
// NOTE THAT WE ARE ASSUMING:
43
//  Main Loop-length is shorter than PWM pulse length. 
44
//    Otherwise, you could recieve stale values in your main loop.
45
// Loop reading rc_values must set rc_available false after reading
38 46
// When our code gets really busy this will become inaccurate,
39 47
// (i believe since micros gets shifted a bit) but for
40 48
// the current application its easy to understand and works very well
......
42 50
// TODO if this starts twitching, also micros has a resolution of 4us.
43 51
static void receiver_on_ail_interrupt() {
44 52
  if(digitalRead(AIL_RECEIVER_PIN) == HIGH) {
45
    start_time[AIL_INDEX] = micros();
53
    // High Received
54
    if((micros() - up_switch_time[AIL_INDEX] > PWM_TIME - PWM_THRESH) &&
55
       (micros() - up_switch_time[AIL_INDEX] < PWM_TIME + PWM_THRESH)) {
56
      // TODO this prevent an instantaneous start-up error, where the
57
      // above condition is true, and up_switch_time 
58
      if((down_switch_time[AIL_INDEX] > up_switch_time[AIL_INDEX]) &&	
59
	 (rc_available[AIL_INDEX] == 0)) {
60
	  rc_value[AIL_INDEX] = (down_switch_time[AIL_INDEX] - 
61
				 up_switch_time[AIL_INDEX]);
62
	  rc_available[AIL_INDEX] = 1;
63
          if(rc_value[AIL_INDEX]/(PWM_TIME/10) >= 1 || rc_value[AIL_INDEX]/(PWM_TIME/21) == 0)
64
            rc_available[AIL_INDEX] == 0;
65
      }
66
    }
67
    up_switch_time[AIL_INDEX] = micros();
46 68
  } else {
47
    if(start_time[AIL_INDEX] && (rc_available[AIL_INDEX] == 0)) {
48
      rc_value[AIL_INDEX] = (micros() - start_time[AIL_INDEX]);
49
      start_time[AIL_INDEX] = 0;
50
      rc_available[AIL_INDEX] = 1;
69
    // Low received
70
    if(up_switch_time[AIL_INDEX]) {
71
      down_switch_time[AIL_INDEX] = micros();
51 72
    }
52 73
  }
53 74
}
54 75

  
55 76
static void receiver_on_thr_interrupt() {
56 77
  if(digitalRead(THR_RECEIVER_PIN) == HIGH) {
57
    start_time[THR_INDEX] = micros();
78
    // High Received
79
    if((micros() - up_switch_time[THR_INDEX] > PWM_TIME - PWM_THRESH) &&
80
       (micros() - up_switch_time[THR_INDEX] < PWM_TIME + PWM_THRESH)) {
81
      // TODO this prevent an instantaneous start-up error, where the
82
      // above condition is true, and up_switch_time 
83
      if((down_switch_time[THR_INDEX] > up_switch_time[THR_INDEX]) &&	
84
	 (rc_available[THR_INDEX] == 0)) {
85
	  rc_value[THR_INDEX] = (down_switch_time[THR_INDEX] - 
86
				 up_switch_time[THR_INDEX]);
87
	  rc_available[THR_INDEX] = 1;
88
          if(rc_value[THR_INDEX]/(PWM_TIME/10) >= 1 || rc_value[THR_INDEX]/(PWM_TIME/21) == 0)
89
            rc_available[THR_INDEX] == 0;
90
      }
91
    }
92
    up_switch_time[THR_INDEX] = micros();
58 93
  } else {
59
    if(start_time[THR_INDEX] && (rc_available[THR_INDEX] == 0)) {
60
      rc_value[THR_INDEX] = (int)(micros() - start_time[THR_INDEX]);
61
      start_time[THR_INDEX] = 0;
62
      rc_available[THR_INDEX] = 1;
94
    // Low received
95
    if(up_switch_time[THR_INDEX]) {
96
      down_switch_time[THR_INDEX] = micros();
63 97
    }
64 98
  }
65 99
}
66 100

  
67 101
// Returns error code
68 102
int receiver_init() {
103
  up_switch_time[0] = 0;
104
  up_switch_time[1] = 0;
105
  down_switch_time[0] = 0;
106
  down_switch_time[1] = 0;
69 107
  attachInterrupt(THR_RECEIVER_INT, receiver_on_thr_interrupt, CHANGE);
70 108
  attachInterrupt(AIL_RECEIVER_INT, receiver_on_ail_interrupt, CHANGE);
71 109
}
......
76 114
// TODO measure throttle positions.
77 115
int receiver_get_angle(int index) {
78 116
  // Math to convert nThrottleIn to 0-180.
79
  int ret_val = (rc_value[index]-AIL_RIGHTMOST)*3/17;
117
  int ret_val = (int)(rc_value[index]-AIL_RIGHTMOST)*3/17;
80 118
  rc_available[index] = 0;
81 119
  return ret_val;
82 120
}
121

  
122

  
123

  
124

  
arduino/RadioBuggyMega/receiver.h
35 35
#endif /* __cplusplus */
36 36

  
37 37
#endif /* _RECEIVER_H_ */
38

  
38

  

Also available in: Unified diff