Marcianise NAS / Mbed 2 deprecated Ilfunzionante

Seguilinea

Dependencies:   HCSR04 X_NUCLEO_IHM12A1 mbed

PLT_DCmotorShield_IHM12A1/MotorShieldIHM12A1.cpp@0:9bd4730782f9, 2018-02-17 (annotated)

Committer:
PaoloGiovanni99
Date:
Sat Feb 17 11:05:43 2018 +0000
Revision:
0:9bd4730782f9
seguilinea funzionante Marcianise

Who changed what in which revision?

UserRevisionLine numberNew contents of line
PaoloGiovanni99 0:9bd4730782f9 1 #include "MotorShieldIHM12A1.h"
PaoloGiovanni99 0:9bd4730782f9 2 #include "mbed.h"
PaoloGiovanni99 0:9bd4730782f9 3
PaoloGiovanni99 0:9bd4730782f9 4
PaoloGiovanni99 0:9bd4730782f9 5 /* Variables -----------------------------------------------------------------*/
PaoloGiovanni99 0:9bd4730782f9 6
PaoloGiovanni99 0:9bd4730782f9 7 /* Initialization parameters of the motor connected to the expansion board. */
PaoloGiovanni99 0:9bd4730782f9 8 Stspin240_250_Init_t initDeviceParameters = {
PaoloGiovanni99 0:9bd4730782f9 9 20000, /* Frequency of PWM of Input Bridge A in Hz up to 100000Hz */
PaoloGiovanni99 0:9bd4730782f9 10 20000, /* Frequency of PWM of Input Bridge B in Hz up to 100000Hz */
PaoloGiovanni99 0:9bd4730782f9 11 20000, /* Frequency of PWM used for Ref pin in Hz up to 100000Hz */
PaoloGiovanni99 0:9bd4730782f9 12 50, /* Duty cycle of PWM used for Ref pin (from 0 to 100) */
PaoloGiovanni99 0:9bd4730782f9 13 TRUE /* Dual Bridge configuration (FALSE for mono, TRUE for dual brush dc) */
PaoloGiovanni99 0:9bd4730782f9 14 };
PaoloGiovanni99 0:9bd4730782f9 15
PaoloGiovanni99 0:9bd4730782f9 16 /* Motor Control Component. */
PaoloGiovanni99 0:9bd4730782f9 17 STSPIN240_250 *motor;
PaoloGiovanni99 0:9bd4730782f9 18
PaoloGiovanni99 0:9bd4730782f9 19 /* Functions -----------------------------------------------------------------*/
PaoloGiovanni99 0:9bd4730782f9 20 void myErrorHandler(uint16_t error)
PaoloGiovanni99 0:9bd4730782f9 21 {
PaoloGiovanni99 0:9bd4730782f9 22 /* Printing to the console. */
PaoloGiovanni99 0:9bd4730782f9 23 printf("Error %d detected\r\n\n", error);
PaoloGiovanni99 0:9bd4730782f9 24
PaoloGiovanni99 0:9bd4730782f9 25 /* Infinite loop */
PaoloGiovanni99 0:9bd4730782f9 26 while(1)
PaoloGiovanni99 0:9bd4730782f9 27 {
PaoloGiovanni99 0:9bd4730782f9 28 }
PaoloGiovanni99 0:9bd4730782f9 29 }
PaoloGiovanni99 0:9bd4730782f9 30
PaoloGiovanni99 0:9bd4730782f9 31 void myFlagIRQHandler(void)
PaoloGiovanni99 0:9bd4730782f9 32 {
PaoloGiovanni99 0:9bd4730782f9 33
PaoloGiovanni99 0:9bd4730782f9 34 printf(" WARNING: \"FLAG\" interrupt triggered.\r\n");
PaoloGiovanni99 0:9bd4730782f9 35
PaoloGiovanni99 0:9bd4730782f9 36 /* Get the state of bridge A */
PaoloGiovanni99 0:9bd4730782f9 37 uint16_t bridgeState = motor->GetBridgeStatus(0);
PaoloGiovanni99 0:9bd4730782f9 38
PaoloGiovanni99 0:9bd4730782f9 39 if (bridgeState == 0)
PaoloGiovanni99 0:9bd4730782f9 40 {
PaoloGiovanni99 0:9bd4730782f9 41 if (motor->GetDeviceState(0) != INACTIVE)
PaoloGiovanni99 0:9bd4730782f9 42 {
PaoloGiovanni99 0:9bd4730782f9 43 /* Bridges were disabled due to overcurrent or over temperature */
PaoloGiovanni99 0:9bd4730782f9 44 /* When motor was running */
PaoloGiovanni99 0:9bd4730782f9 45 myErrorHandler(0XBAD0);
PaoloGiovanni99 0:9bd4730782f9 46 }
PaoloGiovanni99 0:9bd4730782f9 47 }
PaoloGiovanni99 0:9bd4730782f9 48 }
PaoloGiovanni99 0:9bd4730782f9 49
PaoloGiovanni99 0:9bd4730782f9 50 MotorShieldIHM12A1::MotorShieldIHM12A1()
PaoloGiovanni99 0:9bd4730782f9 51 {
PaoloGiovanni99 0:9bd4730782f9 52
PaoloGiovanni99 0:9bd4730782f9 53 //----- Initialization
PaoloGiovanni99 0:9bd4730782f9 54
PaoloGiovanni99 0:9bd4730782f9 55 /* Initializing Motor Control Component. */
PaoloGiovanni99 0:9bd4730782f9 56 #if (defined TARGET_NUCLEO_F030R8)||(defined TARGET_NUCLEO_F334R8)
PaoloGiovanni99 0:9bd4730782f9 57 motor = new STSPIN240_250(D2, D9, D6, D7, D5, D4, A2);
PaoloGiovanni99 0:9bd4730782f9 58 #elif (defined TARGET_NUCLEO_L152RE)
PaoloGiovanni99 0:9bd4730782f9 59 motor = new STSPIN240_250(D2, D9, D6, D7, D5, D4, A3);
PaoloGiovanni99 0:9bd4730782f9 60 #else
PaoloGiovanni99 0:9bd4730782f9 61 motor = new STSPIN240_250(D2, D9, D6, D7, D5, D4, A0);
PaoloGiovanni99 0:9bd4730782f9 62 #endif
PaoloGiovanni99 0:9bd4730782f9 63 if (motor->Init(&initDeviceParameters) != COMPONENT_OK) exit(EXIT_FAILURE);
PaoloGiovanni99 0:9bd4730782f9 64
PaoloGiovanni99 0:9bd4730782f9 65 /* Set dual bridge enabled as two motors are used*/
PaoloGiovanni99 0:9bd4730782f9 66 motor->SetDualFullBridgeConfig(1);
PaoloGiovanni99 0:9bd4730782f9 67
PaoloGiovanni99 0:9bd4730782f9 68 /* Attaching and enabling an interrupt handler. */
PaoloGiovanni99 0:9bd4730782f9 69 motor->AttachFlagIRQ(&myFlagIRQHandler);
PaoloGiovanni99 0:9bd4730782f9 70 motor->EnableFlagIRQ();
PaoloGiovanni99 0:9bd4730782f9 71
PaoloGiovanni99 0:9bd4730782f9 72 /* Attaching an error handler */
PaoloGiovanni99 0:9bd4730782f9 73 motor->AttachErrorHandler(&myErrorHandler);
PaoloGiovanni99 0:9bd4730782f9 74
PaoloGiovanni99 0:9bd4730782f9 75 /* Set PWM Frequency of Ref to 15000 Hz */
PaoloGiovanni99 0:9bd4730782f9 76 motor->SetRefPwmFreq(0, 15000);
PaoloGiovanni99 0:9bd4730782f9 77
PaoloGiovanni99 0:9bd4730782f9 78 /* Set PWM duty cycle of Ref to 60% */
PaoloGiovanni99 0:9bd4730782f9 79 motor->SetRefPwmDc(0, 60);
PaoloGiovanni99 0:9bd4730782f9 80
PaoloGiovanni99 0:9bd4730782f9 81 /* Set PWM Frequency of bridge A inputs to 10000 Hz */
PaoloGiovanni99 0:9bd4730782f9 82 motor->SetBridgeInputPwmFreq(0,10000);
PaoloGiovanni99 0:9bd4730782f9 83
PaoloGiovanni99 0:9bd4730782f9 84 /* Set PWM Frequency of bridge B inputs to 10000 Hz */
PaoloGiovanni99 0:9bd4730782f9 85 motor->SetBridgeInputPwmFreq(1,10000);
PaoloGiovanni99 0:9bd4730782f9 86
PaoloGiovanni99 0:9bd4730782f9 87 }
PaoloGiovanni99 0:9bd4730782f9 88
PaoloGiovanni99 0:9bd4730782f9 89 void MotorShieldIHM12A1::motorSpeed(unsigned int ID, int8_t speed)
PaoloGiovanni99 0:9bd4730782f9 90 {
PaoloGiovanni99 0:9bd4730782f9 91 if (speed > 100) speed = 100;
PaoloGiovanni99 0:9bd4730782f9 92 else if (speed < -100) speed = -100;
PaoloGiovanni99 0:9bd4730782f9 93
PaoloGiovanni99 0:9bd4730782f9 94 if (speed>0)
PaoloGiovanni99 0:9bd4730782f9 95 {
PaoloGiovanni99 0:9bd4730782f9 96 motor->SetSpeed(ID,speed);
PaoloGiovanni99 0:9bd4730782f9 97 motor->Run(ID, BDCMotor::FWD);
PaoloGiovanni99 0:9bd4730782f9 98 }
PaoloGiovanni99 0:9bd4730782f9 99 else if (speed<0)
PaoloGiovanni99 0:9bd4730782f9 100 {
PaoloGiovanni99 0:9bd4730782f9 101 motor->SetSpeed(ID,-speed);
PaoloGiovanni99 0:9bd4730782f9 102 motor->Run(ID, BDCMotor::BWD);
PaoloGiovanni99 0:9bd4730782f9 103 }
PaoloGiovanni99 0:9bd4730782f9 104 else
PaoloGiovanni99 0:9bd4730782f9 105 {
PaoloGiovanni99 0:9bd4730782f9 106 motor->HardStop(ID);
PaoloGiovanni99 0:9bd4730782f9 107 }
PaoloGiovanni99 0:9bd4730782f9 108 }
PaoloGiovanni99 0:9bd4730782f9 109
PaoloGiovanni99 0:9bd4730782f9 110 void MotorShieldIHM12A1::motorStop(unsigned int ID)
PaoloGiovanni99 0:9bd4730782f9 111 {
PaoloGiovanni99 0:9bd4730782f9 112 motor->HardHiZ(ID);
PaoloGiovanni99 0:9bd4730782f9 113 }
PaoloGiovanni99 0:9bd4730782f9 114
PaoloGiovanni99 0:9bd4730782f9 115 void MotorShieldIHM12A1::motorReset()
PaoloGiovanni99 0:9bd4730782f9 116 {
PaoloGiovanni99 0:9bd4730782f9 117 motor->Reset();
PaoloGiovanni99 0:9bd4730782f9 118 }
PaoloGiovanni99 0:9bd4730782f9 119
PaoloGiovanni99 0:9bd4730782f9 120 void MotorShieldIHM12A1::turn(int8_t direction, int8_t speed)
PaoloGiovanni99 0:9bd4730782f9 121 {
PaoloGiovanni99 0:9bd4730782f9 122 bool left = false;
PaoloGiovanni99 0:9bd4730782f9 123 float speedMod;
PaoloGiovanni99 0:9bd4730782f9 124
PaoloGiovanni99 0:9bd4730782f9 125 if (direction < 0)
PaoloGiovanni99 0:9bd4730782f9 126 {
PaoloGiovanni99 0:9bd4730782f9 127 left = true;
PaoloGiovanni99 0:9bd4730782f9 128 direction = -direction;
PaoloGiovanni99 0:9bd4730782f9 129 }
PaoloGiovanni99 0:9bd4730782f9 130
PaoloGiovanni99 0:9bd4730782f9 131 speedMod = ((float)speed/100)*(direction-50)*(-2);
PaoloGiovanni99 0:9bd4730782f9 132
PaoloGiovanni99 0:9bd4730782f9 133 if (left)
PaoloGiovanni99 0:9bd4730782f9 134 {
PaoloGiovanni99 0:9bd4730782f9 135
PaoloGiovanni99 0:9bd4730782f9 136 motorSpeed(1,speedMod);
PaoloGiovanni99 0:9bd4730782f9 137 motorSpeed(0,speed);
PaoloGiovanni99 0:9bd4730782f9 138 }
PaoloGiovanni99 0:9bd4730782f9 139 else
PaoloGiovanni99 0:9bd4730782f9 140 {
PaoloGiovanni99 0:9bd4730782f9 141 motorSpeed(0,speedMod);
PaoloGiovanni99 0:9bd4730782f9 142 motorSpeed(1,speed);
PaoloGiovanni99 0:9bd4730782f9 143 }
PaoloGiovanni99 0:9bd4730782f9 144 }