Project

General

Profile

Revision 0970d303

ID0970d3031aa38bbbab3f63081bb62b00bca66693
Parent b2d280c7
Child 4bdd00ba

Added by Thomas Mullins about 11 years ago

Fixed AVR code for new message locations

Also finished and tested cliffsensors. They work.

View differences:

scout_avr/Makefile
16 16
HDR=src/*.h
17 17
FLAGS=-Isrc/ros_lib -Isrc -mmcu=$(MCU) -DF_CPU=$(F_CPU)UL -funsigned-char -Os -fpack-struct -Wall
18 18

  
19
ROS_MSG_DEPS=rosserial_msgs std_msgs bom sonar headlights cliffsensor
19
ROS_MSG_DEPS=rosserial_msgs std_msgs messages
20 20

  
21 21
default: scout_avr.hex
22 22

  
scout_avr/src/Atmega128rfa1.h
7 7

  
8 8
#define RX_BUFFER_SIZE 256
9 9

  
10
#define MAX_SUBSCRIBERS 6
11
#define MAX_PUBLISHERS 4
10
#define MAX_SUBSCRIBERS 8
11
#define MAX_PUBLISHERS 6
12 12
#define INPUT_SIZE 256
13 13
#define OUTPUT_SIZE 1024
14 14

  
scout_avr/src/bom.h
5 5
#include <stdint.h>
6 6
}
7 7

  
8
#include <bom/bom.h>
8
#include <messages/bom.h>
9 9

  
10 10
// constants for direction
11
#define BOM_FRONT (bom::bom::FRONT)
12
#define BOM_BACK  (bom::bom::BACK)
13
#define BOM_LEFT  (bom::bom::LEFT)
14
#define BOM_RIGHT (bom::bom::RIGHT)
11
#define BOM_FRONT (messages::bom::FRONT)
12
#define BOM_BACK  (messages::bom::BACK)
13
#define BOM_LEFT  (messages::bom::LEFT)
14
#define BOM_RIGHT (messages::bom::RIGHT)
15 15

  
16 16
// timing, in us, for read of valid bits
17 17
#define MIN_LOW_PW 600
scout_avr/src/main.cpp
6 6
#include "stepper.h"
7 7
#include "orb.h"
8 8
#include "cliffSensor.h"
9
#include <bom/bom.h>
10
#include <sonar/sonar_distance.h>
11
#include <sonar/sonar_toggle.h>
12
#include <sonar/sonar_set_scan.h>
13
#include <headlights/set_headlights.h>
14
#include <cliffsensor/cliff_status_changed.h>
9
#include <messages/bom.h>
10
#include <messages/sonar_distance.h>
11
#include <messages/sonar_toggle.h>
12
#include <messages/sonar_set_scan.h>
13
#include <messages/set_headlights.h>
14
#include <messages/cliff_status_changed.h>
15 15
#include <util/delay.h>
16 16

  
17 17
/* Period of main loop in ms */
......
23 23
{
24 24
}
25 25

  
26
void orb_callback(const headlights::set_headlights& msg)
26
void orb_callback(const messages::set_headlights& msg)
27 27
{
28 28
  orb_set0(msg.left_red, msg.left_green, msg.left_blue);
29 29
  orb_set1(msg.right_red, msg.right_green, msg.right_blue);
30 30
}
31 31

  
32 32
/* dammit, Priya, this capitalization just looks ridiculous */
33
void range_toggle_cb(const sonar::sonar_toggleRequest& req,
34
    sonar::sonar_toggleResponse& resp)
33
void range_toggle_cb(const messages::sonar_toggleRequest& req,
34
    messages::sonar_toggleResponse& resp)
35 35
{
36 36
  range_enabled = req.set_on;
37 37
  if (range_enabled)
......
41 41
  resp.ack = true;
42 42
}
43 43

  
44
void range_set_scan_cb(const sonar::sonar_set_scanRequest& req,
45
    sonar::sonar_set_scanResponse& resp)
44
void range_set_scan_cb(const messages::sonar_set_scanRequest& req,
45
    messages::sonar_set_scanResponse& resp)
46 46
{
47 47
  step_sweep_bounds(req.stop_l, req.stop_r);
48 48
  resp.ack = true;
......
53 53
  unsigned long now, next;
54 54
  unsigned int ranges[2];
55 55
  char i, id;
56
  int cliff_front, cliff_left, cliff_right;
56 57

  
57 58
  ros::NodeHandle nh;
58 59
  nh.initNode();
......
66 67

  
67 68
  /* Range */
68 69
  range_init();
69
  sonar::sonar_distance range_msg;
70
  messages::sonar_distance range_msg;
70 71
  ros::Publisher range_pub("sonar_distance", &range_msg);
71 72
  ros::ServiceServer
72
    <sonar::sonar_toggleRequest, sonar::sonar_toggleResponse>
73
    <messages::sonar_toggleRequest, messages::sonar_toggleResponse>
73 74
    range_toggle("sonar_toggle", range_toggle_cb);
74 75
  ros::ServiceServer
75
    <sonar::sonar_set_scanRequest, sonar::sonar_set_scanResponse>
76
    <messages::sonar_set_scanRequest, messages::sonar_set_scanResponse>
76 77
    range_set_scan("sonar_set_scan", range_set_scan_cb);
77 78
  nh.advertise(range_pub);
78 79
  nh.advertiseService(range_toggle);
......
80 81

  
81 82
  /* BOM */
82 83
  bom_init();
83
  bom::bom bom_msg;
84
  messages::bom bom_msg;
84 85
  ros::Publisher bom_pub("bom", &bom_msg);
85 86
  nh.advertise(bom_pub);
86 87

  
87 88
  /* Headlights (aka Orbs) */
88
  //orb_init();
89
  ros::Subscriber<headlights::set_headlights> orb_sub("set_headlights",
89
  orb_init();
90
  /*ros::Subscriber<messages::set_headlights> orb_sub("set_headlights",
90 91
      orb_callback);
91
  nh.subscribe(orb_sub);
92
  nh.subscribe(orb_sub);*/
92 93

  
93 94
  /* Cliff sensors */
94 95
  cliffSensor_init();
95
  cliffsensor::cliff_status_changed cliff_msg;
96
  messages::cliff_status_changed cliff_msg;
96 97
  ros::Publisher cliff_pub("cliff", &cliff_msg);
97 98
  nh.advertise(cliff_pub);
98 99

  
......
143 144
    }
144 145

  
145 146
    /* TODO remove raw values and have single bitmask */
146
    cliff_msg.front_raw = read_cliffSensor_front();
147
    cliff_msg.left_raw = read_cliffSensor_left();
148
    cliff_msg.right_raw = read_cliffSensor_right();
149
    cliff_msg.cliff_status = cliff_msg.front_raw | cliff_msg.left_raw
150
      | cliff_msg.right_raw;
147
    cliff_front = read_cliffSensor_front();
148
    cliff_left = read_cliffSensor_left();
149
    cliff_right = read_cliffSensor_right();
150
    if (cliff_front != cliff_msg.front_raw || cliff_left != cliff_msg.left_raw
151
        || cliff_right != cliff_msg.right_raw) {
152
      cliff_msg.front_raw = cliff_front;
153
      cliff_msg.left_raw = cliff_left;
154
      cliff_msg.right_raw = cliff_right;
155
      cliff_msg.cliff_status = cliff_front || cliff_left || cliff_right;
156
      cliff_pub.publish(&cliff_msg);
157
    }
151 158

  
152 159
  }
153 160

  

Also available in: Unified diff