Tatsuya Hiruma / Mbed 2 deprecated test2

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?

UserRevisionLine numberNew 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>&copy; 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 }