Team DIANA / Mbed OS DC_Serial

.

Dependencies:   X_NUCLEO_IHM04A1 arm_linear_can_2

main.cpp@32:d83629b32b10, 2019-09-15 (annotated)

Committer:
s242743
Date:
Sun Sep 15 02:17:24 2019 +0000
Revision:
32:d83629b32b10
Parent:
30:62e21e5b4445 
.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
stebonicelli 17:dc1b04f0b55d 1 #include "mbed.h"
danielfpq 24:37f139e067b2 2 #include "L6206.h"
s242743 29:93a31c16467b 3 #include "BDCMotor.h"
stebonicelli 26:c18d6aaa474a 4
s242743 32:d83629b32b10 5 #define TX PC_10
s242743 32:d83629b32b10 6 #define RX PC_11
stebonicelli 26:c18d6aaa474a 7
s242743 29:93a31c16467b 8
s242743 32:d83629b32b10 9 Serial SERIAL(D1, D0)
nucleosam 0:36aa6787d4f9 10
danielfpq 24:37f139e067b2 11 static volatile uint16_t gLastError;
danielfpq 24:37f139e067b2 12 static volatile uint8_t gStep = 0;
stebonicelli 17:dc1b04f0b55d 13
danielfpq 24:37f139e067b2 14 L6206_init_t init =
nucleosam 0:36aa6787d4f9 15 {
s242743 29:93a31c16467b 16 L6206_CONF_PARAM_PARALLE_BRIDGES,
danielfpq 24:37f139e067b2 17 {L6206_CONF_PARAM_FREQ_PWM1A, L6206_CONF_PARAM_FREQ_PWM2A, L6206_CONF_PARAM_FREQ_PWM1B, L6206_CONF_PARAM_FREQ_PWM2B},
danielfpq 24:37f139e067b2 18 {100,100,100,100},
gidiana 27:bf0109fb61c3 19 {FORWARD,BACKWARD,FORWARD,BACKWARD},
danielfpq 24:37f139e067b2 20 {INACTIVE,INACTIVE,INACTIVE,INACTIVE},
danielfpq 24:37f139e067b2 21 {FALSE,FALSE}
nucleosam 0:36aa6787d4f9 22 };
stebonicelli 21:533d014f09e0 23
s242743 29:93a31c16467b 24 // Motor definition
s242743 29:93a31c16467b 25 L6206 *LinAct;
s242743 29:93a31c16467b 26 L6206 *EndEff;
stebonicelli 17:dc1b04f0b55d 27
s242743 29:93a31c16467b 28 int speed_elbow = 0;
s242743 29:93a31c16467b 29 int speed_ee = 0;
stebonicelli 21:533d014f09e0 30
s242743 29:93a31c16467b 31 /*********************************/
s242743 29:93a31c16467b 32 /* Interrupt Handlers */
s242743 29:93a31c16467b 33 /*********************************/
s242743 29:93a31c16467b 34
s242743 29:93a31c16467b 35 // Error Handler (called by the library when it reports an error)
danielfpq 24:37f139e067b2 36 void my_error_handler(uint16_t error)
danielfpq 24:37f139e067b2 37 {
danielfpq 24:37f139e067b2 38 /* Backup error number */
danielfpq 24:37f139e067b2 39 gLastError = error;
danielfpq 24:37f139e067b2 40
danielfpq 24:37f139e067b2 41 /* Enter your own code here */
danielfpq 24:37f139e067b2 42 }
danielfpq 24:37f139e067b2 43
s242743 29:93a31c16467b 44 // Flag Handler (overcurrent and thermal alarms reporting)
danielfpq 24:37f139e067b2 45 void my_flag_irq_handler(void)
stebonicelli 17:dc1b04f0b55d 46 {
s242743 29:93a31c16467b 47 /* Get the state of bridge A */
s242743 29:93a31c16467b 48 uint16_t bridgeState = EndEff->get_bridge_status(0);
s242743 29:93a31c16467b 49
s242743 29:93a31c16467b 50 if (bridgeState == 0)
s242743 29:93a31c16467b 51 {
s242743 29:93a31c16467b 52 if ((EndEff->get_device_state(0) != INACTIVE)||
s242743 29:93a31c16467b 53 (EndEff->get_device_state(1) != INACTIVE))
s242743 29:93a31c16467b 54 {
s242743 29:93a31c16467b 55 /* Bridge A was disabling due to overcurrent or over temperature */
s242743 29:93a31c16467b 56 /* When at least on of its motor was running */
s242743 29:93a31c16467b 57 my_error_handler(0XBAD0);
s242743 29:93a31c16467b 58 }
danielfpq 24:37f139e067b2 59 }
danielfpq 24:37f139e067b2 60
s242743 29:93a31c16467b 61 /* Get the state of bridge B */
s242743 29:93a31c16467b 62 bridgeState = LinAct->get_bridge_status(1);
danielfpq 24:37f139e067b2 63
s242743 29:93a31c16467b 64 if (bridgeState == 0)
s242743 29:93a31c16467b 65 {
s242743 29:93a31c16467b 66 if ((LinAct->get_device_state(2) != INACTIVE)||
s242743 29:93a31c16467b 67 (LinAct->get_device_state(3) != INACTIVE))
s242743 29:93a31c16467b 68 {
s242743 29:93a31c16467b 69 /* Bridge A was disabling due to overcurrent or over temperature */
s242743 29:93a31c16467b 70 /* When at least on of its motor was running */
s242743 29:93a31c16467b 71 my_error_handler(0XBAD1);
s242743 29:93a31c16467b 72 }
s242743 29:93a31c16467b 73 }
danielfpq 24:37f139e067b2 74 }
stebonicelli 17:dc1b04f0b55d 75
s242743 32:d83629b32b10 76 /*****************************************************************************/
stebonicelli 17:dc1b04f0b55d 77
s242743 32:d83629b32b10 78 void rx()
s242743 29:93a31c16467b 79 {
s242743 32:d83629b32b10 80
s242743 29:93a31c16467b 81 }
stebonicelli 17:dc1b04f0b55d 82
s242743 29:93a31c16467b 83 /*****************************/
s242743 29:93a31c16467b 84 /* MAIN */
s242743 29:93a31c16467b 85 /*****************************/
s242743 29:93a31c16467b 86
nucleosam 0:36aa6787d4f9 87 int main()
nucleosam 0:36aa6787d4f9 88 {
s242743 29:93a31c16467b 89
s242743 29:93a31c16467b 90 // Motor Initialization
danielfpq 24:37f139e067b2 91 #ifdef TARGET_STM32F429
s242743 29:93a31c16467b 92 LinAct = new L6206(PB_14, PB_15, PA_8, PA_9, PC_6, PC_7); // EN_A, EN_B, IN1_A, IN2_A, IN1_B, IN2_B
s242743 29:93a31c16467b 93 EndEff = new L6206(PB_14, PB_15, PA_8, PA_9, PC_6, PC_7); // EN_A, EN_B, IN1_A, IN2_A, IN1_B, IN2_B
danielfpq 24:37f139e067b2 94 #else
s242743 29:93a31c16467b 95 //LinAct = new L6206(PB_14, PB_15, PA_8, PA_9, PC_6, PC_7);
s242743 29:93a31c16467b 96 LinAct = new L6206(D2, A4, D5, D4, A0, A1);
s242743 29:93a31c16467b 97 EndEff = new L6206(D2, A4, D5, D4, A0, A1);
danielfpq 24:37f139e067b2 98 #endif
nucleosam 0:36aa6787d4f9 99
s242743 29:93a31c16467b 100 if (LinAct->init(&init) != COMPONENT_OK)
s242743 29:93a31c16467b 101 {
s242743 29:93a31c16467b 102 printf("ERROR: vvMotor Init\n\r");
s242743 29:93a31c16467b 103 exit(EXIT_FAILURE);
s242743 29:93a31c16467b 104 }
s242743 29:93a31c16467b 105 if (EndEff->init(&init) != COMPONENT_OK)
s242743 29:93a31c16467b 106 {
s242743 29:93a31c16467b 107 printf("ERROR: vvMotor Init\n\r");
s242743 29:93a31c16467b 108 exit(EXIT_FAILURE);
s242743 29:93a31c16467b 109 }
s242743 29:93a31c16467b 110
s242743 29:93a31c16467b 111 LinAct->attach_flag_interrupt(my_flag_irq_handler);
s242743 29:93a31c16467b 112 LinAct->attach_error_handler(my_error_handler);
s242743 29:93a31c16467b 113 EndEff->attach_flag_interrupt(my_flag_irq_handler);
s242743 29:93a31c16467b 114 EndEff->attach_error_handler(my_error_handler);
s242743 29:93a31c16467b 115 printf("DONE: Motor Init\n\r");
s242743 29:93a31c16467b 116
s242743 29:93a31c16467b 117 LinAct->set_dual_full_bridge_config(PARALLELING_NONE___1_BIDIR_MOTOR_BRIDGE_A__1_BIDIR_MOTOR_BRIDGE_B);
s242743 29:93a31c16467b 118 EndEff->set_dual_full_bridge_config(PARALLELING_NONE___1_BIDIR_MOTOR_BRIDGE_A__1_BIDIR_MOTOR_BRIDGE_B);
danielfpq 24:37f139e067b2 119
s242743 29:93a31c16467b 120 while(true)
s242743 29:93a31c16467b 121 {
s242743 29:93a31c16467b 122 EndEff->set_speed(0, speed_ee);
s242743 29:93a31c16467b 123 if(speed_ee < 0)
s242743 29:93a31c16467b 124 EndEff->run(0, BDCMotor::BWD);
s242743 29:93a31c16467b 125 else if(speed_ee > 0)
s242743 29:93a31c16467b 126 EndEff->run(0, BDCMotor::FWD);
s242743 29:93a31c16467b 127 else if(speed_ee == 0)
s242743 29:93a31c16467b 128 EndEff->hard_stop(0);
s242743 29:93a31c16467b 129
s242743 29:93a31c16467b 130 LinAct->set_speed(1, speed_elbow);
s242743 29:93a31c16467b 131 if(speed_elbow < 0)
s242743 29:93a31c16467b 132 LinAct->run(1, BDCMotor::BWD);
s242743 29:93a31c16467b 133 else if(speed_elbow > 0)
s242743 29:93a31c16467b 134 LinAct->run(1, BDCMotor::FWD);
s242743 29:93a31c16467b 135 else if(speed_elbow == 0)
s242743 29:93a31c16467b 136 LinAct->hard_stop(1);
s242743 29:93a31c16467b 137
s242743 29:93a31c16467b 138 osDelay(100);
s242743 29:93a31c16467b 139 }
s242743 29:93a31c16467b 140 }