Thin wrapper around the pins used to control the Arduino motor shield
Dependents: MUTTv1 ProvaMotoreSeriale SerialComLED motore2 ... more
For an example of using this library, see MUTTv1:
Import programMUTTv1
First (and very, very simple) controller program for the MUTT. See http://mbed.org/users/johnb/notebook/mutt-mbed-enabled-robot-vehicle/
ArduinoMotorShield.cpp
- Committer:
- emuboy
- Date:
- 2014-12-19
- Revision:
- 1:b6bd7c434ab5
- Parent:
- 0:27f8679b31e5
File content as of revision 1:b6bd7c434ab5:
/** Copyright 2014 John Bailey Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. You may obtain a copy of the License at http://www.apache.org/licenses/LICENSE-2.0 Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the License for the specific language governing permissions and limitations under the License. */ #include "ArduinoMotorShield.hpp" #if defined TARGET_KL46Z /* This assumes the presence of the cross-wiring on PWM B (see "Issues" at http://mbed.org/users/johnb/notebook/mutt-mbed-enabled-robot-vehicle/ ) */ #define MOTOR_A_PWM_PIN PTA12 #define MOTOR_B_PWM_PIN PTA5 #define MOTOR_A_BRAKE_PIN PTD2 #define MOTOR_B_BRAKE_PIN PTA13 #define MOTOR_A_CURRENT_PIN PTB0 #define MOTOR_B_CURRENT_PIN PTB1 #define MOTOR_A_DIR_PIN PTD7 #define MOTOR_B_DIR_PIN PTD5 #elif defined TARGET_NUCLEO_F401RE #define MOTOR_A_PWM_PIN PB_3 #define MOTOR_B_PWM_PIN PB_4 #define MOTOR_A_BRAKE_PIN PC_7 #define MOTOR_B_BRAKE_PIN PA_9 #define MOTOR_A_CURRENT_PIN PA_0 #define MOTOR_B_CURRENT_PIN PA_1 #define MOTOR_A_DIR_PIN PA_6 #define MOTOR_B_DIR_PIN PA_5 #else "Error Message: This target's not currently supported! Please look in ArduinoMotorShield.cpp to add." #endif ArduinoMotorShield::ArduinoMotorShield() : m_a_motorControl ( MOTOR_A_PWM_PIN ), m_b_motorControl ( MOTOR_B_PWM_PIN ), m_a_brake ( MOTOR_A_BRAKE_PIN ), m_b_brake ( MOTOR_B_BRAKE_PIN ), m_a_motorCurrent ( MOTOR_A_CURRENT_PIN ), m_b_motorCurrent ( MOTOR_B_CURRENT_PIN ), m_a_motorDirection( MOTOR_A_DIR_PIN ), m_b_motorDirection( MOTOR_B_DIR_PIN ) { m_motorControl[ MOTOR_A ] = &m_a_motorControl; m_motorControl[ MOTOR_B ] = &m_b_motorControl; m_brake[ MOTOR_A ] = &m_a_brake; m_brake[ MOTOR_B ] = &m_b_brake; m_motorCurrent[ MOTOR_A ] = &m_a_motorCurrent; m_motorCurrent[ MOTOR_B ] = &m_b_motorCurrent; m_motorDirection[ MOTOR_A ] = &m_a_motorDirection; m_motorDirection[ MOTOR_B ] = &m_b_motorDirection; m_speed[ MOTOR_A ] = 0; m_speed[ MOTOR_B ] = 0; m_motorForward[ MOTOR_A ] = 0; m_motorForward[ MOTOR_B ] = 0; } void ArduinoMotorShield::SetMotorPower( const Motor_e p_motor, const float p_speed, bool p_force ) { if( p_force || ( p_speed != m_speed[ p_motor ] )) { m_speed[ p_motor ] = p_speed; if( m_speed[ p_motor ] > 0.0f ) { *(m_motorDirection[ p_motor ]) = m_motorForward[ p_motor ]; } else { *(m_motorDirection[ p_motor ]) = !m_motorForward[ p_motor ]; } (m_motorControl[p_motor])->write( fabs( p_speed ) ); } } float ArduinoMotorShield::GetMotorCurrent( const Motor_e p_motor ) { /* AnalogIn returns 0..1. The feedback from the current monitor is 0V = 0A, vref = MAX */ return( *(m_motorCurrent[ p_motor ]) * MOTOR_MAX_CURRENT ); } void ArduinoMotorShield::SetBrake( const Motor_e p_motor, bool p_enable ) { *(m_brake[ p_motor ]) = p_enable; } void ArduinoMotorShield::SetMotorPolarity( const Motor_e p_motor, const MotorDirection_e p_dir ) { if( m_motorForward[ p_motor ] != p_dir ) { m_motorForward[ p_motor ] = p_dir; /* Reset the motor control to take into account the revised direction */ SetMotorPower( p_motor, m_speed[ p_motor ], true ); } }