Shao Rui / Joint_control_2AS5147_DRV8323RH_GR49_HJB_2020_8_10

1

Dependencies:   mbed-dev-f303 FastPWM3

main.cpp@19:bd10a04eedc2, 2017-02-14 (annotated)

Committer:
benkatz
Date:
Tue Feb 14 03:28:16 2017 +0000
Revision:
19:bd10a04eedc2
Parent:
18:f1d56f4acb39
Child:
20:bf9ea5125d52
commit b4 fork; reorganizing coming

Who changed what in which revision?

UserRevisionLine numberNew contents of line
benkatz 17:3c5df2982199 1 const unsigned int BOARDNUM = 0x2;
benkatz 17:3c5df2982199 2
benkatz 17:3c5df2982199 3 //const unsigned int a_id =
benkatz 17:3c5df2982199 4
benkatz 17:3c5df2982199 5 const unsigned int TX_ID = 0x0100;
benkatz 17:3c5df2982199 6
benkatz 18:f1d56f4acb39 7 const unsigned int cmd_ID = (BOARDNUM<<8) + 0x7;
benkatz 18:f1d56f4acb39 8
benkatz 17:3c5df2982199 9
benkatz 17:3c5df2982199 10
benkatz 17:3c5df2982199 11 #include "CANnucleo.h"
benkatz 0:4e1c4df6aabd 12 #include "mbed.h"
benkatz 0:4e1c4df6aabd 13 #include "PositionSensor.h"
benkatz 0:4e1c4df6aabd 14 #include "Inverter.h"
benkatz 0:4e1c4df6aabd 15 #include "SVM.h"
benkatz 0:4e1c4df6aabd 16 #include "FastMath.h"
benkatz 0:4e1c4df6aabd 17 #include "Transforms.h"
benkatz 0:4e1c4df6aabd 18 #include "CurrentRegulator.h"
benkatz 8:10ae7bc88d6e 19 #include "TorqueController.h"
benkatz 9:d7eb815cb057 20 #include "ImpedanceController.h"
benkatz 9:d7eb815cb057 21
benkatz 17:3c5df2982199 22
benkatz 11:c83b18d41e54 23 using namespace FastMath;
benkatz 11:c83b18d41e54 24 using namespace Transforms;
benkatz 9:d7eb815cb057 25
benkatz 17:3c5df2982199 26
benkatz 17:3c5df2982199 27 CANnucleo::CAN can(PB_8, PB_9); // CAN Rx pin name, CAN Tx pin name
benkatz 17:3c5df2982199 28 CANnucleo::CANMessage rxMsg;
benkatz 17:3c5df2982199 29 CANnucleo::CANMessage txMsg;
benkatz 17:3c5df2982199 30 int ledState;
benkatz 17:3c5df2982199 31 int counter = 0;
benkatz 18:f1d56f4acb39 32 int canCmd = 1000;
benkatz 17:3c5df2982199 33 volatile bool msgAvailable = false;
benkatz 17:3c5df2982199 34
benkatz 17:3c5df2982199 35 /**
benkatz 17:3c5df2982199 36 * @brief 'CAN receive-complete' interrup handler.
benkatz 17:3c5df2982199 37 * @note Called on arrival of new CAN message.
benkatz 17:3c5df2982199 38 * Keep it as short as possible.
benkatz 17:3c5df2982199 39 * @param
benkatz 17:3c5df2982199 40 * @retval
benkatz 17:3c5df2982199 41 */
benkatz 17:3c5df2982199 42 void onMsgReceived() {
benkatz 17:3c5df2982199 43 msgAvailable = true;
benkatz 17:3c5df2982199 44 //printf("ping\n\r");
benkatz 17:3c5df2982199 45 }
benkatz 17:3c5df2982199 46
benkatz 17:3c5df2982199 47 void sendCMD(int TX_addr, int val){
benkatz 17:3c5df2982199 48 txMsg.clear(); //clear Tx message storage
benkatz 17:3c5df2982199 49 txMsg.id = TX_addr;
benkatz 17:3c5df2982199 50 txMsg << val;
benkatz 17:3c5df2982199 51 can.write(txMsg);
benkatz 17:3c5df2982199 52 //wait(.1);
benkatz 17:3c5df2982199 53
benkatz 17:3c5df2982199 54 }
benkatz 17:3c5df2982199 55
benkatz 18:f1d56f4acb39 56 void readCAN(void){
benkatz 18:f1d56f4acb39 57 if(msgAvailable) {
benkatz 18:f1d56f4acb39 58 msgAvailable = false; // reset flag for next use
benkatz 18:f1d56f4acb39 59 can.read(rxMsg); // read message into Rx message storage
benkatz 18:f1d56f4acb39 60 // Filtering performed by software:
benkatz 18:f1d56f4acb39 61 if(rxMsg.id == cmd_ID) { // See comments in CAN.cpp for filtering performed by hardware
benkatz 18:f1d56f4acb39 62 rxMsg >> canCmd;
benkatz 18:f1d56f4acb39 63 } // extract first data item
benkatz 18:f1d56f4acb39 64 }
benkatz 18:f1d56f4acb39 65 }
benkatz 17:3c5df2982199 66
benkatz 18:f1d56f4acb39 67 void canLoop(void){
benkatz 18:f1d56f4acb39 68 //printf("%d\n\r", canCmd);
benkatz 18:f1d56f4acb39 69 readCAN();
benkatz 18:f1d56f4acb39 70 //sendCMD(TX_ID, canCmd);
benkatz 18:f1d56f4acb39 71
benkatz 17:3c5df2982199 72 //sendCMD(TX_ID+b_ID, b1);
benkatz 17:3c5df2982199 73 //sendCMD(TX_ID+c_ID, c1);
benkatz 17:3c5df2982199 74 }
benkatz 17:3c5df2982199 75
benkatz 9:d7eb815cb057 76 int id[3] = {0};
benkatz 9:d7eb815cb057 77 float cmd_float[3] = {0.0f};
benkatz 9:d7eb815cb057 78 int raw[3] = {0};
benkatz 11:c83b18d41e54 79 float val_max[3] = {18.0f, 1.0f, 0.1f}; //max angle in radians, stiffness in N-m/rad, damping in N-m*s/rad
benkatz 9:d7eb815cb057 80 int buff[8];
benkatz 9:d7eb815cb057 81 Serial pc(PA_2, PA_3);
benkatz 8:10ae7bc88d6e 82
benkatz 11:c83b18d41e54 83 Inverter inverter(PA_10, PA_9, PA_8, PA_11, 0.02014160156, 0.00005); //hall motor
benkatz 19:bd10a04eedc2 84 PositionSensorAM5147 spi(16384, 4.7, 21); ///1 I really need an eeprom or something to store this....
benkatz 10:370851e6e132 85 //PositionSensorSPI spi(2048, 1.34f, 7); ///2
benkatz 14:80ce59119d93 86
benkatz 9:d7eb815cb057 87
benkatz 15:ef00814e38e2 88 PositionSensorEncoder encoder(4096, 0, 21);
benkatz 10:370851e6e132 89
benkatz 8:10ae7bc88d6e 90
benkatz 11:c83b18d41e54 91
benkatz 15:ef00814e38e2 92 CurrentRegulator foc(&inverter, &spi, &encoder, 0.000033, .005, .55);
benkatz 18:f1d56f4acb39 93 TorqueController torqueController(.082f, &foc);
benkatz 9:d7eb815cb057 94 ImpedanceController impedanceController(&torqueController, &spi, &encoder);
benkatz 9:d7eb815cb057 95
benkatz 7:dc5f27756e02 96 Ticker testing;
benkatz 0:4e1c4df6aabd 97
benkatz 4:c023f7b6f462 98
benkatz 0:4e1c4df6aabd 99
benkatz 0:4e1c4df6aabd 100
benkatz 1:b8bceb4daed5 101 // Current Sampling IRQ
benkatz 2:8724412ad628 102 extern "C" void TIM1_UP_TIM10_IRQHandler(void) {
benkatz 2:8724412ad628 103 if (TIM1->SR & TIM_SR_UIF ) {
benkatz 2:8724412ad628 104 inverter.SampleCurrent();
benkatz 9:d7eb815cb057 105 //foc.Commutate(); ///Putting the loop here doesn't work for some reason. Need to figure out why
benkatz 2:8724412ad628 106 }
benkatz 2:8724412ad628 107 TIM1->SR = 0x0; // reset the status register
benkatz 2:8724412ad628 108 }
benkatz 0:4e1c4df6aabd 109
benkatz 14:80ce59119d93 110 int count = 0;
benkatz 14:80ce59119d93 111 void Loop(void){
benkatz 14:80ce59119d93 112 count++;
benkatz 14:80ce59119d93 113 //impedanceController.SetImpedance(cmd_float[1], cmd_float[2], cmd_float[0]);
benkatz 15:ef00814e38e2 114 //impedanceController.SetImpedance(.1, -0.01, 0);
benkatz 18:f1d56f4acb39 115 float torqueCmd = ((float)(canCmd-1000))/100;
benkatz 18:f1d56f4acb39 116 torqueController.SetTorque(torqueCmd);
benkatz 18:f1d56f4acb39 117 if(count>100){
benkatz 18:f1d56f4acb39 118 canLoop();
benkatz 14:80ce59119d93 119 //float e = spi.GetElecPosition();
benkatz 14:80ce59119d93 120 //float v = encoder.GetMechVelocity();
benkatz 18:f1d56f4acb39 121 //printf("%f\n\r", torqueCmd);
benkatz 14:80ce59119d93 122 //printf("IA: %f IB: %f IC: %f\n\r", inverter.I_A, inverter.I_B, inverter.I_C);
benkatz 14:80ce59119d93 123 count = 0;
benkatz 14:80ce59119d93 124 }
benkatz 11:c83b18d41e54 125
benkatz 0:4e1c4df6aabd 126 }
benkatz 3:6a0015d88d06 127
benkatz 3:6a0015d88d06 128 void PrintStuff(void){
benkatz 14:80ce59119d93 129 //inverter.SetDTC(0.03, 0.0, 0.0);
benkatz 14:80ce59119d93 130
benkatz 9:d7eb815cb057 131 //float v = encoder.GetMechVelocity();
benkatz 8:10ae7bc88d6e 132 //float position = encoder.GetElecPosition();
benkatz 14:80ce59119d93 133 int position = spi.GetRawPosition();
benkatz 9:d7eb815cb057 134 //float m = spi.GetMechPosition();
benkatz 14:80ce59119d93 135 float e = spi.GetElecPosition();
benkatz 19:bd10a04eedc2 136 printf("%f\n\r", e);
benkatz 19:bd10a04eedc2 137 //foc.Commutate();
benkatz 19:bd10a04eedc2 138 //float q = foc.GetQ();
benkatz 19:bd10a04eedc2 139 //printf("position: %d angle: %f q current: %f\n\r", position, e, q);
benkatz 14:80ce59119d93 140 //inverter.getCurrent()
benkatz 9:d7eb815cb057 141 //printf("%f %f %f %f \n\r", m, cmd_float[0], cmd_float[1], cmd_float[2]);
benkatz 9:d7eb815cb057 142 //printf("%d %d %d\n\r", raw[0], raw[1], raw[2]);
benkatz 14:80ce59119d93 143 //printf("IA: %f IB: %f IC: %f\n\r", inverter.I_A, inverter.I_B, inverter.I_C);
benkatz 3:6a0015d88d06 144 }
benkatz 7:dc5f27756e02 145
benkatz 7:dc5f27756e02 146 /*
benkatz 11:c83b18d41e54 147 ////Throw some sines on the phases. useful to make sure the hardware works.
benkatz 7:dc5f27756e02 148 void gen_sine(void){
benkatz 7:dc5f27756e02 149 float f = 1.0f;
benkatz 7:dc5f27756e02 150 float time = t.read();
benkatz 7:dc5f27756e02 151 float a = .45f*sin(6.28318530718f*f*time) + .5f;
benkatz 7:dc5f27756e02 152 float b = .45f*sin(6.28318530718f*f*time + 2.09439510239f) + .5f;
benkatz 7:dc5f27756e02 153 float c = .45f*sin(6.28318530718f*f*time + 4.18879020479f) + .5f;
benkatz 7:dc5f27756e02 154 inverter.SetDTC(a, b, c);
benkatz 7:dc5f27756e02 155 }
benkatz 7:dc5f27756e02 156 */
benkatz 0:4e1c4df6aabd 157
benkatz 0:4e1c4df6aabd 158 int main() {
benkatz 9:d7eb815cb057 159 inverter.DisableInverter();
benkatz 9:d7eb815cb057 160 spi.ZeroPosition();
benkatz 9:d7eb815cb057 161 wait(.1);
benkatz 19:bd10a04eedc2 162 inverter.SetDTC(0.0, 0.0, 0.0);
benkatz 9:d7eb815cb057 163 inverter.EnableInverter();
benkatz 11:c83b18d41e54 164 foc.Reset();
benkatz 14:80ce59119d93 165 testing.attach(&Loop, .000025);
benkatz 18:f1d56f4acb39 166 //canTick.attach(&canLoop, .01);
benkatz 14:80ce59119d93 167 //testing.attach(&PrintStuff, .05);
benkatz 9:d7eb815cb057 168 NVIC_SetPriority(TIM5_IRQn, 2);
benkatz 18:f1d56f4acb39 169
benkatz 18:f1d56f4acb39 170 can.frequency(1000000); // set bit rate to 1Mbps
benkatz 18:f1d56f4acb39 171 can.attach(&onMsgReceived); // attach 'CAN receive-complete' interrupt handler
benkatz 18:f1d56f4acb39 172 can.filter(0x020 << 25, 0xF0000004, CANAny, 0);
benkatz 18:f1d56f4acb39 173
benkatz 14:80ce59119d93 174 pc.baud(921600);
benkatz 18:f1d56f4acb39 175 wait(.1);
benkatz 14:80ce59119d93 176 pc.printf("HobbyKing Cheeta v1.1\n\r");
benkatz 11:c83b18d41e54 177 wait(.1);
benkatz 0:4e1c4df6aabd 178 while(1) {
benkatz 11:c83b18d41e54 179
benkatz 0:4e1c4df6aabd 180 }
benkatz 0:4e1c4df6aabd 181 }