sinyuu masahito
/
yozakura_body
Body control program for Yozakura
main.cpp@0:cd7d5a06068a, 2015-04-24 (annotated)
- Committer:
- masasin
- Date:
- Fri Apr 24 01:56:12 2015 +0000
- Revision:
- 0:cd7d5a06068a
Initial commit.
Who changed what in which revision?
User | Revision | Line number | New 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 | } |