Team DIANA / Mbed OS Basic_DC_Control

Basic code for linear actuator and end effector motor

Dependencies:   QEI X_NUCLEO_IHM04A1 arm_linear_can_2

main.cpp@30:62e21e5b4445, 2019-09-14 (annotated)

Committer:
s242743
Date:
Sat Sep 14 22:30:43 2019 +0000
Revision:
30:62e21e5b4445
Parent:
29:93a31c16467b 
.

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