/************************************************************************** * NAME: Johan Gärtner * * PURPOSE: Driver for the motor signal to certain outputs * * INFORMATION: * * GLOBAL VARIABLES: * * Variable Type Description * * MotorPWMPeriode define The initializing periode of the pwm signal to each motor * * MotorPWMInitPulse define The initializing pulse of the pwm signal to each motor * **************************************************************************/ #include "stm32f4xx.h" #include "stm32f4xx_revo.h" #include "drivers/pwm.h" #include "drivers/motors.h" const int MotorPWMPeriode = 2000; //Micro seconds const int MotorPWMInitPulse = 1000; /* A struct of a pwm motor profile */ typedef struct { GPIO_TypeDef * port; //GPIOA/B/C uint16_t pin; //GPIO_PIN_0/1/2/3/.. TIM_TypeDef * tim; //TIM1/2/3/.. uint32_t channel; //TIM_CHANNEL_1/TIM_CHANNEL_1/.. }motorProfile; /************************************************************************** * BRIEF: Returns the final pulse of a motor driver call * INFORMATION: The pulse is not allowed to be higher then 94 % of the total periode of the pwm signal, otherwise the pwm won't perform correctly * Example - pwmEnableMotor(MOTOR_1) **************************************************************************/ uint16_t checkPulse(uint16_t pulse) { return ((pulse/MotorPWMPeriode)*100 < 94)? pulse: MotorPWMPeriode*0.94; } /************************************************************************** * BRIEF: Returns a profile of a certain motor * INFORMATION: Each Motor has a certain profile which includes a pin, port, timer and a timer channel * Each motor configuration is changed in the "stm32f4xx_revo.h" file * Example - getMotorProfile(MOTOR_1) **************************************************************************/ motorProfile getMotorProfile(uint8_t motor) { motorProfile profile; switch(motor) { case MOTOR_1: profile.port = MOTOR_1_PORT; profile.pin = MOTOR_1_PIN; profile.tim = MOTOR_1_TIM; profile.channel = MOTOR_1_CHANNEL; break; case MOTOR_2: profile.port = MOTOR_2_PORT; profile.pin = MOTOR_2_PIN; profile.tim = MOTOR_2_TIM; profile.channel = MOTOR_2_CHANNEL; break; case MOTOR_3: profile.port = MOTOR_3_PORT; profile.pin = MOTOR_3_PIN; profile.tim = MOTOR_3_TIM; profile.channel = MOTOR_3_CHANNEL; break; case MOTOR_4: profile.port = MOTOR_4_PORT; profile.pin = MOTOR_4_PIN; profile.tim = MOTOR_4_TIM; profile.channel = MOTOR_4_CHANNEL; break; case MOTOR_5: profile.port = MOTOR_5_PORT; profile.pin = MOTOR_5_PIN; profile.tim = MOTOR_5_TIM; profile.channel = MOTOR_5_CHANNEL; break; case MOTOR_6: profile.port = MOTOR_6_PORT; profile.pin = MOTOR_6_PIN; profile.tim = MOTOR_6_TIM; profile.channel = MOTOR_6_CHANNEL; break; case MOTOR_7: profile.port = MOTOR_7_PORT; profile.pin = MOTOR_7_PIN; profile.tim = MOTOR_7_TIM; profile.channel = MOTOR_7_CHANNEL; break; case MOTOR_8: profile.port = MOTOR_8_PORT; profile.pin = MOTOR_8_PIN; profile.tim = MOTOR_8_TIM; profile.channel = MOTOR_8_CHANNEL; break; case MOTOR_9: profile.port = MOTOR_9_PORT; profile.pin = MOTOR_9_PIN; profile.tim = MOTOR_9_TIM; profile.channel = MOTOR_9_CHANNEL; break; case MOTOR_10: profile.port = MOTOR_10_PORT; profile.pin = MOTOR_10_PIN; profile.tim = MOTOR_10_TIM; profile.channel = MOTOR_10_CHANNEL;; break; default: break; } return profile; } /************************************************************************** * BRIEF: Sets the motor output protocol to be either oneshot125 or pwm * INFORMATION: * Example - uint32_t prescaler = setMotorOutput(Oneshot125) *************************************************************************/ uint32_t setMotorOutput(motorOutput motorOutput) { uint32_t prescaler; switch(motorOutput) { case PWM: prescaler = 88; break; case Oneshot125: prescaler = 11; break; default: prescaler = 11; break; } return prescaler; } /************************************************************************** * BRIEF: Initializing a motor (maps a pwm signal to a certain pin) * * INFORMATION: The motor becomes active, Each motor configuration is changed in the "stm32f4xx_revo.h" file * * Example - pwmEnableMotor(MOTOR_1) * **************************************************************************/ void pwmEnableMotor(uint8_t motor, motorOutput motorOutput) { motorProfile profile = getMotorProfile(motor); uint32_t prescaler = setMotorOutput(motorOutput); pwmInit(profile.port, profile.pin, profile.tim, profile.channel, MotorPWMPeriode, MotorPWMInitPulse, prescaler); } /************************************************************************** * BRIEF: Enables all motors (maps a pwm signal to all pins which will be connected to a motor) * * INFORMATION: All motors become active **************************************************************************/ void pwmEnableAllMotors(motorOutput motorOutput) { for (uint8_t i = 1; i < 11; i++ ) pwmEnableMotor(i, motorOutput); } /************************************************************************** * BRIEF: If a motor is Deactivated (no pwm signal), this function will activate the motor to it's last state (the pwm signal will go back to is's last state) * * INFORMATION: * Example - pwmActivateMotor(MOTOR_1) **************************************************************************/ void pwmActivateMotor(uint8_t motor) { motorProfile profile = getMotorProfile(motor); startPwm(profile.channel, profile.tim); } /************************************************************************** * BRIEF: Deactivates a motor (Disables the pwm signal for the motor pin ) * INFORMATION: * Example - pwmDeactivateMotor(MOTOR_1) **************************************************************************/ void pwmDeactivateMotor(uint8_t motor) { motorProfile profile = getMotorProfile(motor); stopPwm(profile.channel, profile.tim); } /************************************************************************** * BRIEF: Activates all motors (Activates a pwm signal to all motor pins to it's last state)* * * INFORMATION: * **************************************************************************/ void pwmActivateAllMotors(void) { for (uint8_t i = 1; i < 11; i++ ) pwmActivateMotor(i); } /************************************************************************** * BRIEF: Deactivates all motors (Deactivate a pwm signal to all motor pins) * * INFORMATION: * * Example - pwmDeactivateAllMotors() * **************************************************************************/ void pwmDeactivateAllMotors(void) { for (uint8_t i = 1; i < 11; i++ ) pwmDeactivateMotor(i); } /************************************************************************** * BRIEF: Change the speed of a certain motor if it's active * * INFORMATION: The speed is changes by switching the the pulse of a pwm signal * * Example - pwmAdjustSpeedOfMotor(MOTOR_2, 200) * **************************************************************************/ void pwmAdjustSpeedOfMotor(uint8_t motor, uint16_t pulse) { motorProfile profile = getMotorProfile(motor); setPwmPulse(profile.channel, profile.tim, checkPulse(pulse)); } /************************************************************************** * BRIEF: Change the speed of a certain motor if it's active * * INFORMATION: The speed is change by switching the the Duty Cycle of a pwm signal (The Duty Cycle may only take values between 0 - 100 %) * * Example - pwmAdjustSpeedOfMotor(MOTOR_2, 50); * **************************************************************************/ void pwmAdjustSpeedOfMotorDutyCycle(uint8_t motor, uint16_t DutyCycle) { uint16_t pulse = (DutyCycle*MotorPWMPeriode)/100; /* Converts the DutyCycle to a pulse */ motorProfile profile = getMotorProfile(motor); setPwmPulse(profile.channel, profile.tim, checkPulse(pulse)); }