Team DIANA / Mbed OS Basic_DC_Control1

Basic code for linear actuator and end effector motor

Dependencies:   QEI X_NUCLEO_IHM04A1 arm_linear_can_2

main.cpp@29:93a31c16467b, 2019-09-14 (annotated)

Committer:
s242743
Date:
Sat Sep 14 20:33:53 2019 +0000
Revision:
29:93a31c16467b
Parent:
28:637b6f726971
Child:
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"
s242743 29:93a31c16467b 4 #include <math.h>
danielfpq 24:37f139e067b2 5
s242743 29:93a31c16467b 6 /****************************/
s242743 29:93a31c16467b 7 /* PIN DEFINITION */
s242743 29:93a31c16467b 8 /****************************/
s242743 29:93a31c16467b 9
s242743 29:93a31c16467b 10 // CAN
s242743 29:93a31c16467b 11 #define CAN_RX PB_8
s242743 29:93a31c16467b 12 #define CAN_TX PB_9
s242743 29:93a31c16467b 13
s242743 29:93a31c16467b 14 // ENCODER
s242743 29:93a31c16467b 15 #define CH_A PA_8
s242743 29:93a31c16467b 16 #define CH_B PA_9
s242743 29:93a31c16467b 17
s242743 29:93a31c16467b 18 /********************************/
s242743 29:93a31c16467b 19 /* CAN */
s242743 29:93a31c16467b 20 /********************************/
stebonicelli 26:c18d6aaa474a 21
s242743 29:93a31c16467b 22 typedef enum
s242743 29:93a31c16467b 23 {
s242743 29:93a31c16467b 24 JOINT_SET_SPEED = 20,
s242743 29:93a31c16467b 25 JOINT_SET_POSITION,
s242743 29:93a31c16467b 26 JOINT_CURRENT_POSITION,
s242743 29:93a31c16467b 27 JOINT_CURRENT_SPEED,
s242743 29:93a31c16467b 28 JOINT_STATUS,
s242743 29:93a31c16467b 29 JOINT_ERROR,
s242743 29:93a31c16467b 30 JOINT_TORQUE,
s242743 29:93a31c16467b 31 JOINT_MAXTORQUE,
s242743 29:93a31c16467b 32 JOINT_ZERO,
s242743 29:93a31c16467b 33 } CAN_COMMANDS;
stebonicelli 26:c18d6aaa474a 34
s242743 29:93a31c16467b 35 typedef enum
s242743 29:93a31c16467b 36 {
s242743 29:93a31c16467b 37 BASE = 1,
s242743 29:93a31c16467b 38 SHOULDER,
s242743 29:93a31c16467b 39 ELBOW,
s242743 29:93a31c16467b 40 WRIST1,
s242743 29:93a31c16467b 41 WRIST2,
s242743 29:93a31c16467b 42 WRIST3,
s242743 29:93a31c16467b 43 END_EFFECTOR,
s242743 29:93a31c16467b 44 CAMERA1,
s242743 29:93a31c16467b 45 CAMERA2,
s242743 29:93a31c16467b 46 } JOINT;
s242743 29:93a31c16467b 47
s242743 29:93a31c16467b 48 //
nucleosam 0:36aa6787d4f9 49
danielfpq 24:37f139e067b2 50 static volatile uint16_t gLastError;
danielfpq 24:37f139e067b2 51 static volatile uint8_t gStep = 0;
stebonicelli 17:dc1b04f0b55d 52
danielfpq 24:37f139e067b2 53 L6206_init_t init =
nucleosam 0:36aa6787d4f9 54 {
s242743 29:93a31c16467b 55 L6206_CONF_PARAM_PARALLE_BRIDGES,
danielfpq 24:37f139e067b2 56 {L6206_CONF_PARAM_FREQ_PWM1A, L6206_CONF_PARAM_FREQ_PWM2A, L6206_CONF_PARAM_FREQ_PWM1B, L6206_CONF_PARAM_FREQ_PWM2B},
danielfpq 24:37f139e067b2 57 {100,100,100,100},
gidiana 27:bf0109fb61c3 58 {FORWARD,BACKWARD,FORWARD,BACKWARD},
danielfpq 24:37f139e067b2 59 {INACTIVE,INACTIVE,INACTIVE,INACTIVE},
danielfpq 24:37f139e067b2 60 {FALSE,FALSE}
nucleosam 0:36aa6787d4f9 61 };
stebonicelli 21:533d014f09e0 62
s242743 29:93a31c16467b 63 // Motor definition
s242743 29:93a31c16467b 64 L6206 *LinAct;
s242743 29:93a31c16467b 65 L6206 *EndEff;
stebonicelli 17:dc1b04f0b55d 66
s242743 29:93a31c16467b 67 int speed_elbow = 0;
s242743 29:93a31c16467b 68 int speed_ee = 0;
stebonicelli 21:533d014f09e0 69
s242743 29:93a31c16467b 70 /*********************************/
s242743 29:93a31c16467b 71 /* Interrupt Handlers */
s242743 29:93a31c16467b 72 /*********************************/
s242743 29:93a31c16467b 73
s242743 29:93a31c16467b 74 // Error Handler (called by the library when it reports an error)
danielfpq 24:37f139e067b2 75 void my_error_handler(uint16_t error)
danielfpq 24:37f139e067b2 76 {
danielfpq 24:37f139e067b2 77 /* Backup error number */
danielfpq 24:37f139e067b2 78 gLastError = error;
danielfpq 24:37f139e067b2 79
danielfpq 24:37f139e067b2 80 /* Enter your own code here */
danielfpq 24:37f139e067b2 81 }
danielfpq 24:37f139e067b2 82
s242743 29:93a31c16467b 83 // Flag Handler (overcurrent and thermal alarms reporting)
danielfpq 24:37f139e067b2 84 void my_flag_irq_handler(void)
stebonicelli 17:dc1b04f0b55d 85 {
s242743 29:93a31c16467b 86 /* Get the state of bridge A */
s242743 29:93a31c16467b 87 uint16_t bridgeState = EndEff->get_bridge_status(0);
s242743 29:93a31c16467b 88
s242743 29:93a31c16467b 89 if (bridgeState == 0)
s242743 29:93a31c16467b 90 {
s242743 29:93a31c16467b 91 if ((EndEff->get_device_state(0) != INACTIVE)||
s242743 29:93a31c16467b 92 (EndEff->get_device_state(1) != INACTIVE))
s242743 29:93a31c16467b 93 {
s242743 29:93a31c16467b 94 /* Bridge A was disabling due to overcurrent or over temperature */
s242743 29:93a31c16467b 95 /* When at least on of its motor was running */
s242743 29:93a31c16467b 96 my_error_handler(0XBAD0);
s242743 29:93a31c16467b 97 }
danielfpq 24:37f139e067b2 98 }
danielfpq 24:37f139e067b2 99
s242743 29:93a31c16467b 100 /* Get the state of bridge B */
s242743 29:93a31c16467b 101 bridgeState = LinAct->get_bridge_status(1);
danielfpq 24:37f139e067b2 102
s242743 29:93a31c16467b 103 if (bridgeState == 0)
s242743 29:93a31c16467b 104 {
s242743 29:93a31c16467b 105 if ((LinAct->get_device_state(2) != INACTIVE)||
s242743 29:93a31c16467b 106 (LinAct->get_device_state(3) != INACTIVE))
s242743 29:93a31c16467b 107 {
s242743 29:93a31c16467b 108 /* Bridge A was disabling due to overcurrent or over temperature */
s242743 29:93a31c16467b 109 /* When at least on of its motor was running */
s242743 29:93a31c16467b 110 my_error_handler(0XBAD1);
s242743 29:93a31c16467b 111 }
s242743 29:93a31c16467b 112 }
danielfpq 24:37f139e067b2 113 }
stebonicelli 17:dc1b04f0b55d 114
s242743 29:93a31c16467b 115 /****************************/
s242743 29:93a31c16467b 116 /* CAN Interface */
s242743 29:93a31c16467b 117 /****************************/
s242743 29:93a31c16467b 118
gidiana 27:bf0109fb61c3 119 CAN can1(PB_8, PB_9); // RX, TX
stebonicelli 17:dc1b04f0b55d 120
s242743 29:93a31c16467b 121 Thread t_canrx, t_cantx;
stebonicelli 17:dc1b04f0b55d 122
s242743 29:93a31c16467b 123 uint32_t gen_can_id(CAN_COMMANDS message_id, JOINT can_id)
stebonicelli 17:dc1b04f0b55d 124 {
s242743 29:93a31c16467b 125 uint32_t id = (uint32_t)can_id; // LSB byte is the controller id.
s242743 29:93a31c16467b 126 id |= (uint32_t)message_id << 8; // Next lowest byte is the packet id.
s242743 29:93a31c16467b 127 id |= 0x80000000; // Send in Extended Frame Format.
s242743 29:93a31c16467b 128 return id;
s242743 29:93a31c16467b 129 }
s242743 29:93a31c16467b 130
s242743 29:93a31c16467b 131 bool can_rx()
s242743 29:93a31c16467b 132 {
s242743 29:93a31c16467b 133 CANMessage messageIn;
s242743 29:93a31c16467b 134 messageIn.format = CANExtended;
s242743 29:93a31c16467b 135 bool status = can1.read(messageIn);
s242743 29:93a31c16467b 136 printf ("CAN ID : %d Message received : %d\n\r", messageIn.id, status);
s242743 29:93a31c16467b 137
s242743 29:93a31c16467b 138 if(can1.read(messageIn) && messageIn.id == gen_can_id(JOINT_SET_SPEED, ELBOW))
danielfpq 24:37f139e067b2 139 {
s242743 29:93a31c16467b 140 speed_elbow = messageIn.data[0] + (messageIn.data[1] << 8) + (messageIn.data[2] << 16) + (messageIn.data[3] << 24);
s242743 29:93a31c16467b 141 }
s242743 29:93a31c16467b 142
s242743 29:93a31c16467b 143 if(can1.read(messageIn) && messageIn.id == gen_can_id(JOINT_SET_SPEED, END_EFFECTOR))
s242743 29:93a31c16467b 144 {
s242743 29:93a31c16467b 145 speed_ee = messageIn.data[0] + (messageIn.data[1] << 8) + (messageIn.data[2] << 16) + (messageIn.data[3] << 24);
s242743 29:93a31c16467b 146 }
s242743 29:93a31c16467b 147
s242743 29:93a31c16467b 148 if(can1.read(messageIn) && messageIn.id == gen_can_id(JOINT_ZERO,ELBOW))
s242743 29:93a31c16467b 149 {
s242743 29:93a31c16467b 150 if((messageIn.data[0] + (messageIn.data[1] << 8) + (messageIn.data[2] << 16) + (messageIn.data[3] << 24)) == 1)
s242743 29:93a31c16467b 151 {
s242743 29:93a31c16467b 152 LinAct->run(1, BDCMotor::BWD);
s242743 29:93a31c16467b 153 }
stebonicelli 26:c18d6aaa474a 154 }
stebonicelli 26:c18d6aaa474a 155
s242743 29:93a31c16467b 156 return status;
nucleosam 0:36aa6787d4f9 157 }
stebonicelli 26:c18d6aaa474a 158
s242743 29:93a31c16467b 159 void can_rx_isr()
s242743 29:93a31c16467b 160 {
s242743 29:93a31c16467b 161 while(1)
s242743 29:93a31c16467b 162 {
s242743 29:93a31c16467b 163 can_rx();
s242743 29:93a31c16467b 164 osDelay(10);
s242743 29:93a31c16467b 165 }
s242743 29:93a31c16467b 166 }
stebonicelli 17:dc1b04f0b55d 167
s242743 29:93a31c16467b 168 /*****************************/
s242743 29:93a31c16467b 169 /* MAIN */
s242743 29:93a31c16467b 170 /*****************************/
s242743 29:93a31c16467b 171
nucleosam 0:36aa6787d4f9 172 int main()
nucleosam 0:36aa6787d4f9 173 {
s242743 29:93a31c16467b 174 can1.frequency(125000);
s242743 29:93a31c16467b 175
s242743 29:93a31c16467b 176 // Motor Initialization
danielfpq 24:37f139e067b2 177 #ifdef TARGET_STM32F429
s242743 29:93a31c16467b 178 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 179 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 180 #else
s242743 29:93a31c16467b 181 //LinAct = new L6206(PB_14, PB_15, PA_8, PA_9, PC_6, PC_7);
s242743 29:93a31c16467b 182 LinAct = new L6206(D2, A4, D5, D4, A0, A1);
s242743 29:93a31c16467b 183 EndEff = new L6206(D2, A4, D5, D4, A0, A1);
danielfpq 24:37f139e067b2 184 #endif
nucleosam 0:36aa6787d4f9 185
s242743 29:93a31c16467b 186 if (LinAct->init(&init) != COMPONENT_OK)
s242743 29:93a31c16467b 187 {
s242743 29:93a31c16467b 188 printf("ERROR: vvMotor Init\n\r");
s242743 29:93a31c16467b 189 exit(EXIT_FAILURE);
s242743 29:93a31c16467b 190 }
s242743 29:93a31c16467b 191 if (EndEff->init(&init) != COMPONENT_OK)
s242743 29:93a31c16467b 192 {
s242743 29:93a31c16467b 193 printf("ERROR: vvMotor Init\n\r");
s242743 29:93a31c16467b 194 exit(EXIT_FAILURE);
s242743 29:93a31c16467b 195 }
s242743 29:93a31c16467b 196
s242743 29:93a31c16467b 197 LinAct->attach_flag_interrupt(my_flag_irq_handler);
s242743 29:93a31c16467b 198 LinAct->attach_error_handler(my_error_handler);
s242743 29:93a31c16467b 199 EndEff->attach_flag_interrupt(my_flag_irq_handler);
s242743 29:93a31c16467b 200 EndEff->attach_error_handler(my_error_handler);
s242743 29:93a31c16467b 201 printf("DONE: Motor Init\n\r");
s242743 29:93a31c16467b 202
s242743 29:93a31c16467b 203 LinAct->set_dual_full_bridge_config(PARALLELING_NONE___1_BIDIR_MOTOR_BRIDGE_A__1_BIDIR_MOTOR_BRIDGE_B);
s242743 29:93a31c16467b 204 EndEff->set_dual_full_bridge_config(PARALLELING_NONE___1_BIDIR_MOTOR_BRIDGE_A__1_BIDIR_MOTOR_BRIDGE_B);
danielfpq 24:37f139e067b2 205
s242743 29:93a31c16467b 206 // CAN Initialization
s242743 29:93a31c16467b 207 t_canrx.start(can_rx_isr);
s242743 29:93a31c16467b 208 printf("DONE: CAN Init\n\r");
s242743 29:93a31c16467b 209
s242743 29:93a31c16467b 210 while(true)
s242743 29:93a31c16467b 211 {
s242743 29:93a31c16467b 212 EndEff->set_speed(0, speed_ee);
s242743 29:93a31c16467b 213 if(speed_ee < 0)
s242743 29:93a31c16467b 214 EndEff->run(0, BDCMotor::BWD);
s242743 29:93a31c16467b 215 else if(speed_ee > 0)
s242743 29:93a31c16467b 216 EndEff->run(0, BDCMotor::FWD);
s242743 29:93a31c16467b 217 else if(speed_ee == 0)
s242743 29:93a31c16467b 218 EndEff->hard_stop(0);
s242743 29:93a31c16467b 219
s242743 29:93a31c16467b 220 LinAct->set_speed(1, speed_elbow);
s242743 29:93a31c16467b 221 if(speed_elbow < 0)
s242743 29:93a31c16467b 222 LinAct->run(1, BDCMotor::BWD);
s242743 29:93a31c16467b 223 else if(speed_elbow > 0)
s242743 29:93a31c16467b 224 LinAct->run(1, BDCMotor::FWD);
s242743 29:93a31c16467b 225 else if(speed_elbow == 0)
s242743 29:93a31c16467b 226 LinAct->hard_stop(1);
s242743 29:93a31c16467b 227
s242743 29:93a31c16467b 228 osDelay(100);
s242743 29:93a31c16467b 229 }
s242743 29:93a31c16467b 230 }