gg
Dependencies: mbed MPU6050 RateLimiter test Math
X_NUCLEO_IHM07M1/SPN7Driver.cpp@8:7efca5258efb, 2020-03-06 (annotated)
- Committer:
- 18fmr36
- Date:
- Fri Mar 06 05:58:45 2020 +0000
- Revision:
- 8:7efca5258efb
gg
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
18fmr36 | 8:7efca5258efb | 1 | /* mbed Microcontroller Library |
18fmr36 | 8:7efca5258efb | 2 | * Copyright (c) 2006-2016 ARM Limited |
18fmr36 | 8:7efca5258efb | 3 | * |
18fmr36 | 8:7efca5258efb | 4 | * Licensed under the Apache License, Version 2.0 (the "License"); |
18fmr36 | 8:7efca5258efb | 5 | * you may not use this file except in compliance with the License. |
18fmr36 | 8:7efca5258efb | 6 | * You may obtain a copy of the License at |
18fmr36 | 8:7efca5258efb | 7 | * |
18fmr36 | 8:7efca5258efb | 8 | * http://www.apache.org/licenses/LICENSE-2.0 |
18fmr36 | 8:7efca5258efb | 9 | * |
18fmr36 | 8:7efca5258efb | 10 | * Unless required by applicable law or agreed to in writing, software |
18fmr36 | 8:7efca5258efb | 11 | * distributed under the License is distributed on an "AS IS" BASIS, |
18fmr36 | 8:7efca5258efb | 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. |
18fmr36 | 8:7efca5258efb | 13 | * See the License for the specific language governing permissions and |
18fmr36 | 8:7efca5258efb | 14 | * limitations under the License. |
18fmr36 | 8:7efca5258efb | 15 | */ |
18fmr36 | 8:7efca5258efb | 16 | |
18fmr36 | 8:7efca5258efb | 17 | /** |
18fmr36 | 8:7efca5258efb | 18 | ****************************************************************************** |
18fmr36 | 8:7efca5258efb | 19 | * @file SPN7Driver.cpp |
18fmr36 | 8:7efca5258efb | 20 | * @author STMicroelectronics |
18fmr36 | 8:7efca5258efb | 21 | * @brief Implementation of SPN7Driver class |
18fmr36 | 8:7efca5258efb | 22 | ****************************************************************************** |
18fmr36 | 8:7efca5258efb | 23 | * @copy |
18fmr36 | 8:7efca5258efb | 24 | * |
18fmr36 | 8:7efca5258efb | 25 | * THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS |
18fmr36 | 8:7efca5258efb | 26 | * WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE |
18fmr36 | 8:7efca5258efb | 27 | * TIME. AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY |
18fmr36 | 8:7efca5258efb | 28 | * DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING |
18fmr36 | 8:7efca5258efb | 29 | * FROM THE CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE |
18fmr36 | 8:7efca5258efb | 30 | * CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS. |
18fmr36 | 8:7efca5258efb | 31 | * |
18fmr36 | 8:7efca5258efb | 32 | * <h2><center>© COPYRIGHT 2016 STMicroelectronics</center></h2> |
18fmr36 | 8:7efca5258efb | 33 | */ |
18fmr36 | 8:7efca5258efb | 34 | |
18fmr36 | 8:7efca5258efb | 35 | // This example is based on the BLDCmotorDriver motor control library |
18fmr36 | 8:7efca5258efb | 36 | // by the TVZ Mechatronics Team, University of Applied Sciences Zagreb, |
18fmr36 | 8:7efca5258efb | 37 | // Professional Study in Mechatronics: |
18fmr36 | 8:7efca5258efb | 38 | // https://developer.mbed.org/teams/TVZ-Mechatronics-Team/code/BLDCmotorDriver/ |
18fmr36 | 8:7efca5258efb | 39 | |
18fmr36 | 8:7efca5258efb | 40 | #include "mbed.h" |
18fmr36 | 8:7efca5258efb | 41 | #include "SPN7Driver.h" |
18fmr36 | 8:7efca5258efb | 42 | |
18fmr36 | 8:7efca5258efb | 43 | typedef enum { |
18fmr36 | 8:7efca5258efb | 44 | ST_BLDC_LOW = 0, |
18fmr36 | 8:7efca5258efb | 45 | ST_BLDC_HIGH, |
18fmr36 | 8:7efca5258efb | 46 | ST_BLDC_OFF |
18fmr36 | 8:7efca5258efb | 47 | } st_bldc_status_t; |
18fmr36 | 8:7efca5258efb | 48 | |
18fmr36 | 8:7efca5258efb | 49 | /**************************************************************************/ |
18fmr36 | 8:7efca5258efb | 50 | /** |
18fmr36 | 8:7efca5258efb | 51 | @brief Constructor |
18fmr36 | 8:7efca5258efb | 52 | * @param pIN1 Logic input pin IN1 of L6230 chip |
18fmr36 | 8:7efca5258efb | 53 | * @param pIN2 Logic input pin IN2 of L6230 chip |
18fmr36 | 8:7efca5258efb | 54 | * @param pIN3 Logic input pin IN3 of L6230 chip |
18fmr36 | 8:7efca5258efb | 55 | * @param pEN1 Enable channel pin EN1 of L6230 chip |
18fmr36 | 8:7efca5258efb | 56 | * @param pEN2 Enable channel pin EN2 of L6230 chip |
18fmr36 | 8:7efca5258efb | 57 | * @param pEN3 Enable channel pin EN3 of L6230 chip |
18fmr36 | 8:7efca5258efb | 58 | * @param pH1 Hall sensor pin for phase #1 (A) of X-NUCLEO-IHM07M1 |
18fmr36 | 8:7efca5258efb | 59 | * @param pH2 Hall sensor pin for phase #2 (B) of X-NUCLEO-IHM07M1 |
18fmr36 | 8:7efca5258efb | 60 | * @param pH3 Hall sensor pin for phase #3 (Z) of X-NUCLEO-IHM07M1 |
18fmr36 | 8:7efca5258efb | 61 | * @param pFault Fault LED pin of X-NUCLEO-IHM07M1 |
18fmr36 | 8:7efca5258efb | 62 | */ |
18fmr36 | 8:7efca5258efb | 63 | /**************************************************************************/ |
18fmr36 | 8:7efca5258efb | 64 | SPN7Driver::SPN7Driver(PinName pIN1, PinName pIN2, PinName pIN3, |
18fmr36 | 8:7efca5258efb | 65 | PinName pEN1, PinName pEN2, PinName pEN3, |
18fmr36 | 8:7efca5258efb | 66 | PinName pH1, PinName pH2, PinName pH3, |
18fmr36 | 8:7efca5258efb | 67 | PinName pC1, PinName pC2, PinName pC3, |
18fmr36 | 8:7efca5258efb | 68 | PinName pFault) : |
18fmr36 | 8:7efca5258efb | 69 | BLDCmotorDriver(pIN1, pIN2, pIN3, |
18fmr36 | 8:7efca5258efb | 70 | pEN1, pEN2, pEN3, |
18fmr36 | 8:7efca5258efb | 71 | pH1, pH2, pH3, |
18fmr36 | 8:7efca5258efb | 72 | pC1, pC2, pC3, |
18fmr36 | 8:7efca5258efb | 73 | pFault) |
18fmr36 | 8:7efca5258efb | 74 | { |
18fmr36 | 8:7efca5258efb | 75 | // The BLDCmotorDriver class was implemented for half-bridge drivers |
18fmr36 | 8:7efca5258efb | 76 | // so the pin names may be misleading when referring to the L6230 chip. |
18fmr36 | 8:7efca5258efb | 77 | // Get pointers to each input pin and call them IN[x] (logic input) |
18fmr36 | 8:7efca5258efb | 78 | // to be consistent with the terminology used in the L6230 documentation. |
18fmr36 | 8:7efca5258efb | 79 | PwmOut* IN[3] = {&GH_A, &GH_B, &GH_C}; |
18fmr36 | 8:7efca5258efb | 80 | |
18fmr36 | 8:7efca5258efb | 81 | // Set the switching period of the INx logic input pins (PWM driven) |
18fmr36 | 8:7efca5258efb | 82 | for (int i = 0; i < 3; i++) { |
18fmr36 | 8:7efca5258efb | 83 | IN[i]->period(switchingPeriod); |
18fmr36 | 8:7efca5258efb | 84 | } |
18fmr36 | 8:7efca5258efb | 85 | |
18fmr36 | 8:7efca5258efb | 86 | // Set the step commutation function (triggered by the Hall sensors) |
18fmr36 | 8:7efca5258efb | 87 | H1.rise(this, &SPN7Driver::commutation); |
18fmr36 | 8:7efca5258efb | 88 | H2.rise(this, &SPN7Driver::commutation); |
18fmr36 | 8:7efca5258efb | 89 | H3.rise(this, &SPN7Driver::commutation); |
18fmr36 | 8:7efca5258efb | 90 | H1.fall(this, &SPN7Driver::commutation); |
18fmr36 | 8:7efca5258efb | 91 | H2.fall(this, &SPN7Driver::commutation); |
18fmr36 | 8:7efca5258efb | 92 | H3.fall(this, &SPN7Driver::commutation); |
18fmr36 | 8:7efca5258efb | 93 | } |
18fmr36 | 8:7efca5258efb | 94 | |
18fmr36 | 8:7efca5258efb | 95 | /**************************************************************************/ |
18fmr36 | 8:7efca5258efb | 96 | /** |
18fmr36 | 8:7efca5258efb | 97 | @brief Set duty cycle for motor control |
18fmr36 | 8:7efca5258efb | 98 | * @param dc duty cycle value (>0 clockwise; <0 anti-clockwise) |
18fmr36 | 8:7efca5258efb | 99 | */ |
18fmr36 | 8:7efca5258efb | 100 | /**************************************************************************/ |
18fmr36 | 8:7efca5258efb | 101 | void SPN7Driver::setDutyCycle(float dc) { |
18fmr36 | 8:7efca5258efb | 102 | if (dc >= -1 && dc <= 1) { |
18fmr36 | 8:7efca5258efb | 103 | ticker.attach(this, &SPN7Driver::commutation, sampleTime); |
18fmr36 | 8:7efca5258efb | 104 | tempDutyCycle = dc; |
18fmr36 | 8:7efca5258efb | 105 | } else { |
18fmr36 | 8:7efca5258efb | 106 | coast(); |
18fmr36 | 8:7efca5258efb | 107 | } |
18fmr36 | 8:7efca5258efb | 108 | } |
18fmr36 | 8:7efca5258efb | 109 | |
18fmr36 | 8:7efca5258efb | 110 | // six-step phase commutation |
18fmr36 | 8:7efca5258efb | 111 | void SPN7Driver::commutation() |
18fmr36 | 8:7efca5258efb | 112 | { |
18fmr36 | 8:7efca5258efb | 113 | // The BLDCmotorDriver class was implemented for half-bridge drivers |
18fmr36 | 8:7efca5258efb | 114 | // so the pin names may be misleading when referring to the L6230 chip. |
18fmr36 | 8:7efca5258efb | 115 | // Get pointers to each input pin and call them IN[x] (logic input) |
18fmr36 | 8:7efca5258efb | 116 | // to be consistent with the terminology used in the L6230 documentation. |
18fmr36 | 8:7efca5258efb | 117 | PwmOut* IN[3] = {&GH_A, &GH_B, &GH_C}; |
18fmr36 | 8:7efca5258efb | 118 | // Get pointers to each enable pin and call them ENx (enable channel) |
18fmr36 | 8:7efca5258efb | 119 | // to be consistent with the terminology used in the L6230 documentation. |
18fmr36 | 8:7efca5258efb | 120 | DigitalOut* EN[3] = {&GL_A, &GL_B, &GL_C}; |
18fmr36 | 8:7efca5258efb | 121 | |
18fmr36 | 8:7efca5258efb | 122 | st_bldc_status_t tab[6][3] = { |
18fmr36 | 8:7efca5258efb | 123 | {ST_BLDC_OFF, ST_BLDC_LOW, ST_BLDC_HIGH}, |
18fmr36 | 8:7efca5258efb | 124 | {ST_BLDC_HIGH, ST_BLDC_LOW, ST_BLDC_OFF}, |
18fmr36 | 8:7efca5258efb | 125 | {ST_BLDC_HIGH, ST_BLDC_OFF, ST_BLDC_LOW}, |
18fmr36 | 8:7efca5258efb | 126 | {ST_BLDC_OFF, ST_BLDC_HIGH, ST_BLDC_LOW}, |
18fmr36 | 8:7efca5258efb | 127 | {ST_BLDC_LOW, ST_BLDC_HIGH, ST_BLDC_OFF}, |
18fmr36 | 8:7efca5258efb | 128 | {ST_BLDC_LOW, ST_BLDC_OFF, ST_BLDC_HIGH}, |
18fmr36 | 8:7efca5258efb | 129 | }; |
18fmr36 | 8:7efca5258efb | 130 | |
18fmr36 | 8:7efca5258efb | 131 | dutyCycle = rl.out(tempDutyCycle); |
18fmr36 | 8:7efca5258efb | 132 | int sector = getSector(); |
18fmr36 | 8:7efca5258efb | 133 | |
18fmr36 | 8:7efca5258efb | 134 | if (dutyCycle == 0) { |
18fmr36 | 8:7efca5258efb | 135 | // Stop the motor |
18fmr36 | 8:7efca5258efb | 136 | coast(); |
18fmr36 | 8:7efca5258efb | 137 | return; |
18fmr36 | 8:7efca5258efb | 138 | } |
18fmr36 | 8:7efca5258efb | 139 | |
18fmr36 | 8:7efca5258efb | 140 | // Move to next sector (i.e. commute phase) |
18fmr36 | 8:7efca5258efb | 141 | if (dutyCycle > 0 ) { |
18fmr36 | 8:7efca5258efb | 142 | // Clockwise spinning |
18fmr36 | 8:7efca5258efb | 143 | |
18fmr36 | 8:7efca5258efb | 144 | // The rows of the phase status table are offset by one when spinning |
18fmr36 | 8:7efca5258efb | 145 | // clockwise so we need to increment the current sector by two steps |
18fmr36 | 8:7efca5258efb | 146 | sector = (sector + 2) % 6; |
18fmr36 | 8:7efca5258efb | 147 | |
18fmr36 | 8:7efca5258efb | 148 | } else { |
18fmr36 | 8:7efca5258efb | 149 | // Anti-clockwise spinning |
18fmr36 | 8:7efca5258efb | 150 | sector--; |
18fmr36 | 8:7efca5258efb | 151 | if(sector < 0) sector = 5; |
18fmr36 | 8:7efca5258efb | 152 | } |
18fmr36 | 8:7efca5258efb | 153 | |
18fmr36 | 8:7efca5258efb | 154 | // Get the absolute value of the duty cycle for the PWM |
18fmr36 | 8:7efca5258efb | 155 | float d = (dutyCycle > 0) ? dutyCycle : -dutyCycle; |
18fmr36 | 8:7efca5258efb | 156 | |
18fmr36 | 8:7efca5258efb | 157 | // Update the logic inputs and the enable pins |
18fmr36 | 8:7efca5258efb | 158 | for (int i = 0; i < 3; i++) { |
18fmr36 | 8:7efca5258efb | 159 | *EN[i] = (tab[sector][i] == ST_BLDC_OFF) ? 0 : 1; |
18fmr36 | 8:7efca5258efb | 160 | *IN[i] = (tab[sector][i] == ST_BLDC_HIGH) ? d : 0; |
18fmr36 | 8:7efca5258efb | 161 | } |
18fmr36 | 8:7efca5258efb | 162 | } |