sinyuu masahito / Mbed 2 deprecated yozakura_body

Body control program for Yozakura

Dependencies:   mbed

main.cpp@1:5b504250b032, 2015-04-28 (annotated)

Committer:
masasin
Date:
Tue Apr 28 06:17:16 2015 +0000
Revision:
1:5b504250b032
Parent:
0:cd7d5a06068a 
Update

Who changed what in which revision?

UserRevisionLine numberNew contents of line
masasin 0:cd7d5a06068a 1 // (C) 2015 Kyoto University Mechatronics Laboratory
masasin 0:cd7d5a06068a 2 // Released under the GNU General Public License, version 3
masasin 0:cd7d5a06068a 3 #include "mbed.h"
masasin 0:cd7d5a06068a 4
masasin 0:cd7d5a06068a 5 Serial rpi(USBTX, USBRX); // USB port acts as a serial connection with rpi.
masasin 0:cd7d5a06068a 6
masasin 0:cd7d5a06068a 7 // A bitfield representing the motor packet received from the rpi.
masasin 0:cd7d5a06068a 8 //
masasin 0:cd7d5a06068a 9 // The first two bits of the packet represent the motor ID, between 0 and 3.
masasin 0:cd7d5a06068a 10 // The third bit is the sign, with a value of 1 when negative and 0 when
masasin 0:cd7d5a06068a 11 // positive. The last five bits represent the speed, with a value between 0
masasin 0:cd7d5a06068a 12 // and 31. [0:31] corresponds to a [0:1] requested speed.
masasin 0:cd7d5a06068a 13 //
masasin 0:cd7d5a06068a 14 // If the sign is 1 and the speed is zero, the packet can instead be used for
masasin 0:cd7d5a06068a 15 // running up to four functions based on the Motor ID.
masasin 0:cd7d5a06068a 16 //
masasin 0:cd7d5a06068a 17 // Note that bitfields on the mbed are little endian by default.
masasin 0:cd7d5a06068a 18 struct MotorPacketBits {
masasin 0:cd7d5a06068a 19 unsigned int motor_id : 2;
masasin 0:cd7d5a06068a 20 unsigned int negative : 1;
masasin 0:cd7d5a06068a 21 unsigned int speed : 5;
masasin 0:cd7d5a06068a 22 };
masasin 0:cd7d5a06068a 23
masasin 0:cd7d5a06068a 24 union MotorPacket {
masasin 0:cd7d5a06068a 25 struct MotorPacketBits b;
masasin 0:cd7d5a06068a 26 unsigned char as_byte;
masasin 0:cd7d5a06068a 27 };
masasin 0:cd7d5a06068a 28
masasin 0:cd7d5a06068a 29
masasin 0:cd7d5a06068a 30 // Class representing a motor driver.
masasin 0:cd7d5a06068a 31 //
masasin 0:cd7d5a06068a 32 // Connect the PWM and DIR pins to the mbed. The motor driver's fault signals
masasin 0:cd7d5a06068a 33 // should go to the Raspberry Pi. Ground can be connected to either.
masasin 0:cd7d5a06068a 34 //
masasin 0:cd7d5a06068a 35 // Datasheet: https://www.pololu.com/product/755
masasin 0:cd7d5a06068a 36 //
masasin 0:cd7d5a06068a 37 // Examples:
masasin 0:cd7d5a06068a 38 // Motor motor(p21, p11);
masasin 0:cd7d5a06068a 39 // motor.drive(0.5) // Runs motor forward at 50% speed.
masasin 0:cd7d5a06068a 40 // motor.drive(-0.5) // Runs motor backwards at 50% speed.
masasin 0:cd7d5a06068a 41 class Motor {
masasin 0:cd7d5a06068a 42 public:
masasin 0:cd7d5a06068a 43 // Initialize the motor.
masasin 0:cd7d5a06068a 44 //
masasin 0:cd7d5a06068a 45 // Parameters:
masasin 0:cd7d5a06068a 46 // pin_pwm: The motor driver's PWM pin. In order to have PWM output, the
masasin 0:cd7d5a06068a 47 // pin should be between 21 and 26.
masasin 0:cd7d5a06068a 48 // pin_dir: The motor driver's DIR pin. If the driver is not connected in
masasin 0:cd7d5a06068a 49 // reverse, HI is forward, and LO is reverse.
masasin 0:cd7d5a06068a 50 // reversed: Whether the motor driver's DIR pin is connected in reverse.
masasin 0:cd7d5a06068a 51 Motor(PinName pin_pwm, PinName pin_dir, bool reversed)
masasin 0:cd7d5a06068a 52 : pwm_(pin_pwm), dir_(pin_dir), reversed_(reversed) {
masasin 0:cd7d5a06068a 53 pwm_ = 0;
masasin 0:cd7d5a06068a 54 dir_ = 0;
masasin 0:cd7d5a06068a 55 pwm_.period_us(40); // Set PWM output frequency to 25 kHz.
masasin 0:cd7d5a06068a 56 }
masasin 0:cd7d5a06068a 57
masasin 0:cd7d5a06068a 58 // Drive the motor at the given speed.
masasin 0:cd7d5a06068a 59 //
masasin 0:cd7d5a06068a 60 // Parameters:
masasin 0:cd7d5a06068a 61 // speed: A value between -1 and 1, representing the motor speed.
masasin 0:cd7d5a06068a 62 void Drive(float speed) {
masasin 0:cd7d5a06068a 63 if (reversed_) {
masasin 0:cd7d5a06068a 64 dir_ = speed < 0 ? 1 : 0;
masasin 0:cd7d5a06068a 65 } else {
masasin 0:cd7d5a06068a 66 dir_ = speed < 0 ? 0 : 1;
masasin 0:cd7d5a06068a 67 }
masasin 0:cd7d5a06068a 68 pwm_ = abs(speed);
masasin 0:cd7d5a06068a 69 }
masasin 0:cd7d5a06068a 70
masasin 0:cd7d5a06068a 71 private:
masasin 0:cd7d5a06068a 72 PwmOut pwm_; // The motor driver's PWM pin.
masasin 0:cd7d5a06068a 73 DigitalOut dir_; // The motor driver's DIR pin.
masasin 0:cd7d5a06068a 74 bool reversed_; // Whether DIR is connected in reverse.
masasin 0:cd7d5a06068a 75 };
masasin 0:cd7d5a06068a 76
masasin 0:cd7d5a06068a 77
masasin 0:cd7d5a06068a 78 int main() {
masasin 0:cd7d5a06068a 79 // The four motors are in an array. The raspberry pi expects this order; do
masasin 0:cd7d5a06068a 80 // not change it without changing the code for the RPi as well.
masasin 0:cd7d5a06068a 81 Motor motors[4] = { Motor(p26, p27, false), // Left wheels
masasin 0:cd7d5a06068a 82 Motor(p25, p28, true), // Right wheels
masasin 0:cd7d5a06068a 83 Motor(p24, p29, true), // Left flipper
masasin 0:cd7d5a06068a 84 Motor(p23, p30, false) }; // Right flipper
masasin 0:cd7d5a06068a 85
masasin 0:cd7d5a06068a 86 AnalogIn adcs[6] = { p15, p16, p17, p18, // Unused
masasin 0:cd7d5a06068a 87 p19, // Left flipper position
masasin 0:cd7d5a06068a 88 p20 }; // Right flipper position
masasin 0:cd7d5a06068a 89
masasin 0:cd7d5a06068a 90 int n_adc = 2; // Number of ADC Channels in use. Max is 6.
masasin 0:cd7d5a06068a 91 uint16_t adc_results[n_adc];
masasin 0:cd7d5a06068a 92 for (int i = 0; i < n_adc; i++) {
masasin 0:cd7d5a06068a 93 adc_results[i] = 0; // Zero the results.
masasin 0:cd7d5a06068a 94 }
masasin 0:cd7d5a06068a 95
masasin 0:cd7d5a06068a 96 union MotorPacket packet;
masasin 0:cd7d5a06068a 97 int sign;
masasin 0:cd7d5a06068a 98
masasin 0:cd7d5a06068a 99 rpi.baud(38400); // Match this in the RPi settings.
masasin 0:cd7d5a06068a 100
masasin 0:cd7d5a06068a 101 while (1) {
masasin 0:cd7d5a06068a 102 // Get packet from RPi.
masasin 0:cd7d5a06068a 103 packet.as_byte = rpi.getc();
masasin 0:cd7d5a06068a 104
masasin 0:cd7d5a06068a 105 if (packet.b.motor_id == 3 and packet.b.negative and not packet.b.speed) {
masasin 0:cd7d5a06068a 106 rpi.printf("body\n");
masasin 0:cd7d5a06068a 107 } else {
masasin 0:cd7d5a06068a 108 // Drive motor.
masasin 0:cd7d5a06068a 109 sign = packet.b.negative ? -1 : 1;
masasin 0:cd7d5a06068a 110 motors[packet.b.motor_id].Drive(sign * packet.b.speed / 31.0);
masasin 0:cd7d5a06068a 111
masasin 0:cd7d5a06068a 112 // Update flipper positions.
masasin 0:cd7d5a06068a 113 adc_results[n_adc - 2] = adcs[4].read_u16(); // Left flipper position
masasin 0:cd7d5a06068a 114 adc_results[n_adc - 1] = adcs[5].read_u16(); // Right flipper position
masasin 0:cd7d5a06068a 115
masasin 0:cd7d5a06068a 116 // Send data to RPi.
masasin 0:cd7d5a06068a 117 for (int i = 0; i < n_adc; i++) {
masasin 0:cd7d5a06068a 118 rpi.printf("%X ", adc_results[i]);
masasin 0:cd7d5a06068a 119 }
masasin 0:cd7d5a06068a 120 rpi.printf("\n");
masasin 0:cd7d5a06068a 121 }
masasin 0:cd7d5a06068a 122 }
masasin 0:cd7d5a06068a 123 }