Revision 8913c26d scout/motors/src/motors.cpp

View differences:

scout/motors/src/motors.cpp
57 57
Motor::Motor(int pwm_gpt, int in1_gpio, int in2_gpio)
58 58
{
59 59
  char buf[60];
60
  int pwm, in1, in2;
61 60

  
62 61
  // open device files
63 62
  sprintf(buf, "/dev/pwm%d", pwm_gpt);
64
  fpwm.open(buf);
63
  fpwm.open(buf, ios::out);
65 64
  sprintf(buf, "/sys/class/gpio/gpio%d/value", in1_gpio);
66
  fin1.open(buf);
65
  fin1.open(buf, ios::out);
67 66
  sprintf(buf, "/sys/class/gpio/gpio%d/value", in2_gpio);
68
  fin2.open(buf);
69

  
70
  // read current values from device files
71
  fpwm >> pwm;
72
  fin1 >> in1;
73
  fin2 >> in2;
67
  fin2.open(buf, ios::out);
74 68

  
75
  // convert to our speed units
76
  if (in1)
77
  {
78
    if (in2)
79
    {
80
      // short brake
81
      speed = 0;
82
    }
83
    else
84
    {
85
      // CW
86
      speed = pwm;
87
    }
88
  }
89
  else
90
  {
91
    if (in2)
92
    {
93
      // CCW
94
      speed = -pwm;
95
    }
96
    else
97
    {
98
      // off
99
      speed = 0;
100
    }
101
  }
69
  /* I set speed to 0 here rather than reading in what the current speed is
70
   * because the pwm driver does not support seeking in the device files. Doing
71
   * both reading and writing causes fpwm to attempt seeking, which fails,
72
   * causing fpwm to refuse to do any more io. So, we can only write.
73
   */
74
  speed = 0;
102 75

  
103
  // ensure we are in a consistent state by writing back to hardware
76
  // ensure we are in a consistent state by writing to hardware
104 77
  set_speed(speed);
105 78
}
106 79

  
107 80
/**
81
 * @brief Motor destructor
82
 *
83
 * Sets the motor's speed to zero in preparation for the node exiting
84
 */
85
Motor::~Motor()
86
{
87
    set_speed(0);
88
}
89

  
90
/**
108 91
 * @brief Returns current motor speed
109 92
 *
110 93
 * Note that this doesn't read from the hardware
......
156 139
  fin2 << in2 << flush;
157 140
}
158 141

  
159
/// @todo change these to the correct motor locations
142
/// @todo change these to the correct motor locations / directions
160 143
// Motor state variables
161 144
static Motor motor_fl( 8, 70, 71);
162 145
static Motor motor_fr( 9, 72, 73);
......
241 224
    ROS_INFO("Motors node ready.");
242 225
    ros::spin();
243 226

  
227
    motor_fl.set_speed(0);
228
    motor_fr.set_speed(0);
229
    motor_bl.set_speed(0);
230
    motor_br.set_speed(0);
231

  
244 232
    return 0;
245 233
}
246 234

  

Also available in: Unified diff