Diff: main.cpp

Revision:

0:cd7d5a06068a

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Apr 24 01:56:12 2015 +0000
@@ -0,0 +1,123 @@
+// (C) 2015 Kyoto University Mechatronics Laboratory
+// Released under the GNU General Public License, version 3
+#include "mbed.h"
+
+Serial rpi(USBTX, USBRX); // USB port acts as a serial connection with rpi.
+
+// A bitfield representing the motor packet received from the rpi.
+//
+// The first two bits of the packet represent the motor ID, between 0 and 3.
+// The third bit is the sign, with a value of 1 when negative and 0 when
+// positive. The last five bits represent the speed, with a value between 0
+// and 31. [0:31] corresponds to a [0:1] requested speed.
+//
+// If the sign is 1 and the speed is zero, the packet can instead be used for
+// running up to four functions based on the Motor ID.
+//
+// Note that bitfields on the mbed are little endian by default.
+struct MotorPacketBits {
+                        unsigned int motor_id : 2;
+                        unsigned int negative : 1;
+                        unsigned int speed : 5;
+                       };
+
+union MotorPacket {
+                   struct MotorPacketBits b;
+                   unsigned char as_byte;
+                  };
+
+
+// Class representing a motor driver.
+//
+// Connect the PWM and DIR pins to the mbed. The motor driver's fault signals
+// should go to the Raspberry Pi. Ground can be connected to either.
+//
+// Datasheet: https://www.pololu.com/product/755
+//
+// Examples:
+//     Motor motor(p21, p11);
+//     motor.drive(0.5) // Runs motor forward at 50% speed.
+//     motor.drive(-0.5) // Runs motor backwards at 50% speed.
+class Motor {
+ public:
+  // Initialize the motor.
+  //
+  // Parameters:
+  //   pin_pwm: The motor driver's PWM pin. In order to have PWM output, the
+  //            pin should be between 21 and 26.
+  //   pin_dir: The motor driver's DIR pin. If the driver is not connected in
+  //            reverse, HI is forward, and LO is reverse.
+  //   reversed: Whether the motor driver's DIR pin is connected in reverse.
+  Motor(PinName pin_pwm, PinName pin_dir, bool reversed)
+      : pwm_(pin_pwm), dir_(pin_dir), reversed_(reversed) {
+    pwm_ = 0;
+    dir_ = 0;
+    pwm_.period_us(40);  // Set PWM output frequency to 25 kHz.
+  }
+
+  // Drive the motor at the given speed.
+  //
+  // Parameters:
+  //   speed: A value between -1 and 1, representing the motor speed.
+  void Drive(float speed) {
+    if (reversed_) {
+      dir_ = speed < 0 ? 1 : 0;
+    } else {
+      dir_ = speed < 0 ? 0 : 1;
+    }
+    pwm_ = abs(speed);
+  }
+
+ private:
+    PwmOut pwm_;      // The motor driver's PWM pin.
+    DigitalOut dir_;  // The motor driver's DIR pin.
+    bool reversed_;   // Whether DIR is connected in reverse.
+};
+
+
+int main() {
+  // The four motors are in an array. The raspberry pi expects this order; do
+  // not change it without changing the code for the RPi as well.
+  Motor motors[4] = { Motor(p26, p27, false),     // Left wheels
+                      Motor(p25, p28, true),      // Right wheels
+                      Motor(p24, p29, true),      // Left flipper
+                      Motor(p23, p30, false) };   // Right flipper
+
+  AnalogIn adcs[6] = { p15, p16, p17, p18,   // Unused
+                       p19,                  // Left flipper position
+                       p20 };                // Right flipper position
+
+  int n_adc = 2; // Number of ADC Channels in use. Max is 6.
+  uint16_t adc_results[n_adc];
+  for (int i = 0; i < n_adc; i++) {
+    adc_results[i] = 0; // Zero the results.
+  }
+
+  union MotorPacket packet;
+  int sign;
+
+  rpi.baud(38400);  // Match this in the RPi settings.
+
+  while (1) {
+    // Get packet from RPi.
+    packet.as_byte = rpi.getc();
+
+    if (packet.b.motor_id == 3 and packet.b.negative and not packet.b.speed) {
+      rpi.printf("body\n");
+    } else {
+      // Drive motor.
+      sign = packet.b.negative ? -1 : 1;
+      motors[packet.b.motor_id].Drive(sign * packet.b.speed / 31.0);
+
+      // Update flipper positions.
+      adc_results[n_adc - 2] = adcs[4].read_u16();  // Left flipper position
+      adc_results[n_adc - 1] = adcs[5].read_u16();  // Right flipper position
+
+      // Send data to RPi.
+      for (int i = 0; i < n_adc; i++) {
+        rpi.printf("%X ", adc_results[i]);
+      }
+      rpi.printf("\n");
+    }
+  }
+}
\ No newline at end of file