Milos Milosavljevic / ArduinoMotorShield

Motor Drive UH

Fork of ArduinoMotorShield by John Bailey

ArduinoMotorShield.cpp@2:1dec2d3a899c, 2016-11-28 (annotated)

Committer:
milosm
Date:
Mon Nov 28 11:35:22 2016 +0000
Revision:
2:1dec2d3a899c
Parent:
0:27f8679b31e5 
this is a test for motor drive

Who changed what in which revision?

UserRevisionLine numberNew contents of line
johnb 0:27f8679b31e5 1 /**
johnb 0:27f8679b31e5 2
johnb 0:27f8679b31e5 3 Copyright 2014 John Bailey
johnb 0:27f8679b31e5 4
johnb 0:27f8679b31e5 5 Licensed under the Apache License, Version 2.0 (the "License");
johnb 0:27f8679b31e5 6 you may not use this file except in compliance with the License.
johnb 0:27f8679b31e5 7 You may obtain a copy of the License at
johnb 0:27f8679b31e5 8
johnb 0:27f8679b31e5 9 http://www.apache.org/licenses/LICENSE-2.0
johnb 0:27f8679b31e5 10
johnb 0:27f8679b31e5 11 Unless required by applicable law or agreed to in writing, software
johnb 0:27f8679b31e5 12 distributed under the License is distributed on an "AS IS" BASIS,
johnb 0:27f8679b31e5 13 WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
johnb 0:27f8679b31e5 14 See the License for the specific language governing permissions and
johnb 0:27f8679b31e5 15 limitations under the License.
johnb 0:27f8679b31e5 16
johnb 0:27f8679b31e5 17 */
johnb 0:27f8679b31e5 18
johnb 0:27f8679b31e5 19 #include "ArduinoMotorShield.hpp"
johnb 0:27f8679b31e5 20
milosm 2:1dec2d3a899c 21 #if defined TARGET_KL25Z
johnb 0:27f8679b31e5 22
johnb 0:27f8679b31e5 23 /* This assumes the presence of the cross-wiring on PWM B
johnb 0:27f8679b31e5 24 (see "Issues" at http://mbed.org/users/johnb/notebook/mutt-mbed-enabled-robot-vehicle/ )
johnb 0:27f8679b31e5 25 */
johnb 0:27f8679b31e5 26 #define MOTOR_A_PWM_PIN PTA12
johnb 0:27f8679b31e5 27 #define MOTOR_B_PWM_PIN PTA5
johnb 0:27f8679b31e5 28
milosm 2:1dec2d3a899c 29 #define MOTOR_A_BRAKE_PIN PTD5
johnb 0:27f8679b31e5 30 #define MOTOR_B_BRAKE_PIN PTA13
johnb 0:27f8679b31e5 31
johnb 0:27f8679b31e5 32 #define MOTOR_A_CURRENT_PIN PTB0
johnb 0:27f8679b31e5 33 #define MOTOR_B_CURRENT_PIN PTB1
johnb 0:27f8679b31e5 34
milosm 2:1dec2d3a899c 35 #define MOTOR_A_DIR_PIN PTD3
milosm 2:1dec2d3a899c 36 #define MOTOR_B_DIR_PIN PTD1
johnb 0:27f8679b31e5 37
johnb 0:27f8679b31e5 38 #else
johnb 0:27f8679b31e5 39
johnb 0:27f8679b31e5 40 "Error Message: This target's not currently supported! Please look in ArduinoMotorShield.cpp to add."
johnb 0:27f8679b31e5 41
johnb 0:27f8679b31e5 42 #endif
johnb 0:27f8679b31e5 43
johnb 0:27f8679b31e5 44 ArduinoMotorShield::ArduinoMotorShield() : m_a_motorControl ( MOTOR_A_PWM_PIN ), m_b_motorControl ( MOTOR_B_PWM_PIN ),
johnb 0:27f8679b31e5 45 m_a_brake ( MOTOR_A_BRAKE_PIN ), m_b_brake ( MOTOR_B_BRAKE_PIN ),
johnb 0:27f8679b31e5 46 m_a_motorCurrent ( MOTOR_A_CURRENT_PIN ), m_b_motorCurrent ( MOTOR_B_CURRENT_PIN ),
johnb 0:27f8679b31e5 47 m_a_motorDirection( MOTOR_A_DIR_PIN ), m_b_motorDirection( MOTOR_B_DIR_PIN )
johnb 0:27f8679b31e5 48 {
johnb 0:27f8679b31e5 49 m_motorControl[ MOTOR_A ] = &m_a_motorControl;
johnb 0:27f8679b31e5 50 m_motorControl[ MOTOR_B ] = &m_b_motorControl;
johnb 0:27f8679b31e5 51 m_brake[ MOTOR_A ] = &m_a_brake;
johnb 0:27f8679b31e5 52 m_brake[ MOTOR_B ] = &m_b_brake;
johnb 0:27f8679b31e5 53 m_motorCurrent[ MOTOR_A ] = &m_a_motorCurrent;
johnb 0:27f8679b31e5 54 m_motorCurrent[ MOTOR_B ] = &m_b_motorCurrent;
johnb 0:27f8679b31e5 55 m_motorDirection[ MOTOR_A ] = &m_a_motorDirection;
johnb 0:27f8679b31e5 56 m_motorDirection[ MOTOR_B ] = &m_b_motorDirection;
johnb 0:27f8679b31e5 57 m_speed[ MOTOR_A ] = 0;
johnb 0:27f8679b31e5 58 m_speed[ MOTOR_B ] = 0;
johnb 0:27f8679b31e5 59 m_motorForward[ MOTOR_A ] = 0;
johnb 0:27f8679b31e5 60 m_motorForward[ MOTOR_B ] = 0;
johnb 0:27f8679b31e5 61 }
johnb 0:27f8679b31e5 62
johnb 0:27f8679b31e5 63 void ArduinoMotorShield::SetMotorPower( const Motor_e p_motor, const float p_speed, bool p_force )
johnb 0:27f8679b31e5 64 {
johnb 0:27f8679b31e5 65 if( p_force || ( p_speed != m_speed[ p_motor ] ))
johnb 0:27f8679b31e5 66 {
johnb 0:27f8679b31e5 67 m_speed[ p_motor ] = p_speed;
johnb 0:27f8679b31e5 68
johnb 0:27f8679b31e5 69 if( m_speed[ p_motor ] > 0.0f )
johnb 0:27f8679b31e5 70 {
johnb 0:27f8679b31e5 71 *(m_motorDirection[ p_motor ]) = m_motorForward[ p_motor ];
johnb 0:27f8679b31e5 72 } else {
johnb 0:27f8679b31e5 73 *(m_motorDirection[ p_motor ]) = !m_motorForward[ p_motor ];
johnb 0:27f8679b31e5 74 }
johnb 0:27f8679b31e5 75
johnb 0:27f8679b31e5 76 (m_motorControl[p_motor])->write( fabs( p_speed ) );
johnb 0:27f8679b31e5 77 }
johnb 0:27f8679b31e5 78 }
johnb 0:27f8679b31e5 79
johnb 0:27f8679b31e5 80 float ArduinoMotorShield::GetMotorCurrent( const Motor_e p_motor )
johnb 0:27f8679b31e5 81 {
johnb 0:27f8679b31e5 82 /* AnalogIn returns 0..1. The feedback from the current monitor is 0V = 0A, vref = MAX */
johnb 0:27f8679b31e5 83 return( *(m_motorCurrent[ p_motor ]) * MOTOR_MAX_CURRENT );
johnb 0:27f8679b31e5 84 }
johnb 0:27f8679b31e5 85
johnb 0:27f8679b31e5 86 void ArduinoMotorShield::SetBrake( const Motor_e p_motor, bool p_enable )
johnb 0:27f8679b31e5 87 {
johnb 0:27f8679b31e5 88 *(m_brake[ p_motor ]) = p_enable;
johnb 0:27f8679b31e5 89 }
johnb 0:27f8679b31e5 90
johnb 0:27f8679b31e5 91 void ArduinoMotorShield::SetMotorPolarity( const Motor_e p_motor, const MotorDirection_e p_dir )
johnb 0:27f8679b31e5 92 {
johnb 0:27f8679b31e5 93 if( m_motorForward[ p_motor ] != p_dir )
johnb 0:27f8679b31e5 94 {
johnb 0:27f8679b31e5 95 m_motorForward[ p_motor ] = p_dir;
johnb 0:27f8679b31e5 96 /* Reset the motor control to take into account the revised direction */
johnb 0:27f8679b31e5 97 SetMotorPower( p_motor, m_speed[ p_motor ], true );
johnb 0:27f8679b31e5 98 }
johnb 0:27f8679b31e5 99 }