TZU-CHING CHEN / Mbed 2 deprecated LDSC_Motor_Upper_Control

Dependencies:   mbed

main.cpp@0:654bd6e4a0f9, 2022-04-18 (annotated)

Committer:
TzuChingChen
Date:
Mon Apr 18 04:35:04 2022 +0000
Revision:
0:654bd6e4a0f9
LDSC_Motor_Upper_Control

Who changed what in which revision?

UserRevisionLine numberNew contents of line
TzuChingChen 0:654bd6e4a0f9 1 #include "mbed.h"
TzuChingChen 0:654bd6e4a0f9 2 #include "main.h"
TzuChingChen 0:654bd6e4a0f9 3 #include <stdint.h>
TzuChingChen 0:654bd6e4a0f9 4 #include <math.h>
TzuChingChen 0:654bd6e4a0f9 5
TzuChingChen 0:654bd6e4a0f9 6 /*
TzuChingChen 0:654bd6e4a0f9 7 * IO pins declaration
TzuChingChen 0:654bd6e4a0f9 8 */
TzuChingChen 0:654bd6e4a0f9 9 AnalogIn Trq_raw(PC_1);
TzuChingChen 0:654bd6e4a0f9 10
TzuChingChen 0:654bd6e4a0f9 11 /*
TzuChingChen 0:654bd6e4a0f9 12 * Mbed Objects declaration
TzuChingChen 0:654bd6e4a0f9 13 */
TzuChingChen 0:654bd6e4a0f9 14 Serial pc(USBTX, USBRX, 115200);
TzuChingChen 0:654bd6e4a0f9 15 CAN can1(PB_8, PB_9, 1000000); //1Mbps, contain critical torque command message
TzuChingChen 0:654bd6e4a0f9 16 Ticker ticker1; //100Hz task
TzuChingChen 0:654bd6e4a0f9 17 CANMessage can_msg_Rx;
TzuChingChen 0:654bd6e4a0f9 18 CANMessage can_msg_Tx;
TzuChingChen 0:654bd6e4a0f9 19
TzuChingChen 0:654bd6e4a0f9 20
TzuChingChen 0:654bd6e4a0f9 21 //Timers
TzuChingChen 0:654bd6e4a0f9 22 void timer1_interrupt(void)
TzuChingChen 0:654bd6e4a0f9 23 {
TzuChingChen 0:654bd6e4a0f9 24 HSTick += 1;
TzuChingChen 0:654bd6e4a0f9 25 LSTick += 1;
TzuChingChen 0:654bd6e4a0f9 26
TzuChingChen 0:654bd6e4a0f9 27 if (HSTick > 9) // 100Hz
TzuChingChen 0:654bd6e4a0f9 28 {
TzuChingChen 0:654bd6e4a0f9 29 HST_EXFL = 1;
TzuChingChen 0:654bd6e4a0f9 30 HSTick = 0;
TzuChingChen 0:654bd6e4a0f9 31 }
TzuChingChen 0:654bd6e4a0f9 32 if (LSTick > 99) // 10Hz
TzuChingChen 0:654bd6e4a0f9 33 {
TzuChingChen 0:654bd6e4a0f9 34 LST_EXFL = 1;
TzuChingChen 0:654bd6e4a0f9 35 LSTick = 0;
TzuChingChen 0:654bd6e4a0f9 36 }
TzuChingChen 0:654bd6e4a0f9 37 }
TzuChingChen 0:654bd6e4a0f9 38
TzuChingChen 0:654bd6e4a0f9 39 int main()
TzuChingChen 0:654bd6e4a0f9 40 {
TzuChingChen 0:654bd6e4a0f9 41 // Initializing
TzuChingChen 0:654bd6e4a0f9 42 printf("Motor_upper_control starts up\n");
TzuChingChen 0:654bd6e4a0f9 43
TzuChingChen 0:654bd6e4a0f9 44 //Init CAN network
TzuChingChen 0:654bd6e4a0f9 45 CAN_init(); // Note now in Gloable test mode only for testing 2019/11/17
TzuChingChen 0:654bd6e4a0f9 46
TzuChingChen 0:654bd6e4a0f9 47 //attach IRQs after all other things are ready
TzuChingChen 0:654bd6e4a0f9 48 ticker1.attach_us(&timer1_interrupt, 1000); //1 ms Systick
TzuChingChen 0:654bd6e4a0f9 49 while(1) {
TzuChingChen 0:654bd6e4a0f9 50
TzuChingChen 0:654bd6e4a0f9 51 if(HST_EXFL) {
TzuChingChen 0:654bd6e4a0f9 52 HST_EXFL = false;
TzuChingChen 0:654bd6e4a0f9 53 // pc.printf("High speed says hi! \n");
TzuChingChen 0:654bd6e4a0f9 54 Tx_High_CAN1();
TzuChingChen 0:654bd6e4a0f9 55 }
TzuChingChen 0:654bd6e4a0f9 56 if(LST_EXFL) {
TzuChingChen 0:654bd6e4a0f9 57 LST_EXFL = false;
TzuChingChen 0:654bd6e4a0f9 58 // pc.printf("Low speed says hi! \n");
TzuChingChen 0:654bd6e4a0f9 59 printf("Torque read: %.1f ", float(Trq_raw));
TzuChingChen 0:654bd6e4a0f9 60 Tx_Low_CAN1();
TzuChingChen 0:654bd6e4a0f9 61 }
TzuChingChen 0:654bd6e4a0f9 62 }
TzuChingChen 0:654bd6e4a0f9 63 }
TzuChingChen 0:654bd6e4a0f9 64
TzuChingChen 0:654bd6e4a0f9 65 void CAN_init(void)
TzuChingChen 0:654bd6e4a0f9 66 {
TzuChingChen 0:654bd6e4a0f9 67 //Set CAN system
TzuChingChen 0:654bd6e4a0f9 68 SET_BIT(CAN1->MCR, CAN_MCR_ABOM); // Enable auto reboot after bus off
TzuChingChen 0:654bd6e4a0f9 69 // can1.filter(Trq_send_ID,0xFFFF,CANStandard,0); // ID filter listing mode
TzuChingChen 0:654bd6e4a0f9 70 // can1.filter(Id_cmd_ID,0xFFFF,CANStandard,1);
TzuChingChen 0:654bd6e4a0f9 71 // can1.filter(Iq_cmd_ID,0xFFFF,CANStandard,2);
TzuChingChen 0:654bd6e4a0f9 72 // can1.mode(CAN::GlobalTest); // Add only for testing 2019/11/13
TzuChingChen 0:654bd6e4a0f9 73 can1.attach(&Rx_CAN1, CAN::RxIrq); // CAN1 Recieve Irq
TzuChingChen 0:654bd6e4a0f9 74 }
TzuChingChen 0:654bd6e4a0f9 75
TzuChingChen 0:654bd6e4a0f9 76 void Rx_CAN1(void)
TzuChingChen 0:654bd6e4a0f9 77 {
TzuChingChen 0:654bd6e4a0f9 78 // LED = 1;
TzuChingChen 0:654bd6e4a0f9 79 int16_t tmp;
TzuChingChen 0:654bd6e4a0f9 80
TzuChingChen 0:654bd6e4a0f9 81 if(can1.read(can_msg_Rx)) {
TzuChingChen 0:654bd6e4a0f9 82 // switch(can_msg_Rx.id) { //Filtered input message
TzuChingChen 0:654bd6e4a0f9 83 // Start of 100Hz msg
TzuChingChen 0:654bd6e4a0f9 84 // case Some_ID://1
TzuChingChen 0:654bd6e4a0f9 85 // //HSB from FL motor drive
TzuChingChen 0:654bd6e4a0f9 86 // Something1 = can_msg_Rx.data[6] & 0x03; //Get DSM_STAT
TzuChingChen 0:654bd6e4a0f9 87 // tmp = can_msg_Rx.data[5] << 8 | can_msg_Rx.data[4];
TzuChingChen 0:654bd6e4a0f9 88 // Something2 = tmp*1.0f;
TzuChingChen 0:654bd6e4a0f9 89 // tmp = can_msg_Rx.data[3] << 8 | can_msg_Rx.data[2];
TzuChingChen 0:654bd6e4a0f9 90 // Something3 = tmp * 0.01f;
TzuChingChen 0:654bd6e4a0f9 91 // tmp = can_msg_Rx.data[1] << 8 | can_msg_Rx.data[0];
TzuChingChen 0:654bd6e4a0f9 92 // Something4 = tmp * 0.01f;
TzuChingChen 0:654bd6e4a0f9 93 // break;
TzuChingChen 0:654bd6e4a0f9 94 // }
TzuChingChen 0:654bd6e4a0f9 95 }
TzuChingChen 0:654bd6e4a0f9 96 // LED = 0;
TzuChingChen 0:654bd6e4a0f9 97 }
TzuChingChen 0:654bd6e4a0f9 98
TzuChingChen 0:654bd6e4a0f9 99 void Tx_High_CAN1(void) // 100 Hz
TzuChingChen 0:654bd6e4a0f9 100 {
TzuChingChen 0:654bd6e4a0f9 101 int16_t tmp;
TzuChingChen 0:654bd6e4a0f9 102 tmp = (int16_t) (Id_cmd * 100.0f);
TzuChingChen 0:654bd6e4a0f9 103 temp_msg[0] = tmp;
TzuChingChen 0:654bd6e4a0f9 104 temp_msg[1] = tmp >> 8U;
TzuChingChen 0:654bd6e4a0f9 105 temp_msg[2] = 0U;
TzuChingChen 0:654bd6e4a0f9 106 tmp = (int16_t) (Iq_cmd * 100.0f);
TzuChingChen 0:654bd6e4a0f9 107 temp_msg[3] = tmp;
TzuChingChen 0:654bd6e4a0f9 108 temp_msg[4] = tmp >> 8U;
TzuChingChen 0:654bd6e4a0f9 109 temp_msg[5] = 0U;
TzuChingChen 0:654bd6e4a0f9 110 temp_msg[6] = 0U;
TzuChingChen 0:654bd6e4a0f9 111 temp_msg[7] = 0U;
TzuChingChen 0:654bd6e4a0f9 112 can_msg_Tx = CANMessage(Trq_send_ID,temp_msg,8,CANData,CANStandard);
TzuChingChen 0:654bd6e4a0f9 113 CANpendTX();
TzuChingChen 0:654bd6e4a0f9 114 can1.write(can_msg_Tx);
TzuChingChen 0:654bd6e4a0f9 115 }
TzuChingChen 0:654bd6e4a0f9 116
TzuChingChen 0:654bd6e4a0f9 117 void Tx_Low_CAN1(void) // 10 Hz
TzuChingChen 0:654bd6e4a0f9 118 {
TzuChingChen 0:654bd6e4a0f9 119 int16_t tmp;
TzuChingChen 0:654bd6e4a0f9 120 tmp = (int16_t) (Trq_send * 100.0f);
TzuChingChen 0:654bd6e4a0f9 121 temp_msg[0] = tmp;
TzuChingChen 0:654bd6e4a0f9 122 temp_msg[1] = tmp >> 8U;
TzuChingChen 0:654bd6e4a0f9 123 temp_msg[2] = 0U;
TzuChingChen 0:654bd6e4a0f9 124 temp_msg[3] = 0U;
TzuChingChen 0:654bd6e4a0f9 125 temp_msg[4] = 0U;
TzuChingChen 0:654bd6e4a0f9 126 temp_msg[5] = 0U;
TzuChingChen 0:654bd6e4a0f9 127 temp_msg[6] = 0U;
TzuChingChen 0:654bd6e4a0f9 128 temp_msg[7] = 0U;
TzuChingChen 0:654bd6e4a0f9 129 can_msg_Tx = CANMessage(Trq_send_ID,temp_msg,8,CANData,CANStandard);
TzuChingChen 0:654bd6e4a0f9 130 CANpendTX();
TzuChingChen 0:654bd6e4a0f9 131 can1.write(can_msg_Tx);
TzuChingChen 0:654bd6e4a0f9 132 }
TzuChingChen 0:654bd6e4a0f9 133 void CANpendTX(void)
TzuChingChen 0:654bd6e4a0f9 134 {
TzuChingChen 0:654bd6e4a0f9 135 //Pend till TX box has empty slot, timeout will generate error
TzuChingChen 0:654bd6e4a0f9 136 uint32_t timeout = 0;
TzuChingChen 0:654bd6e4a0f9 137 while(!(CAN1->TSR & CAN_TSR_TME_Msk)) {
TzuChingChen 0:654bd6e4a0f9 138 //Wait till non empty
TzuChingChen 0:654bd6e4a0f9 139 timeout += 1;
TzuChingChen 0:654bd6e4a0f9 140 if(timeout > 10000) {
TzuChingChen 0:654bd6e4a0f9 141 // Put some timeout error handler
TzuChingChen 0:654bd6e4a0f9 142 break;
TzuChingChen 0:654bd6e4a0f9 143 }
TzuChingChen 0:654bd6e4a0f9 144 }
TzuChingChen 0:654bd6e4a0f9 145 }