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/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