nhan pham an / Hobbyking_Cheetah_Compact_DRV8323_StepDir

Fork and fix for mwork

Dependencies:   mbed-dev-f303 FastPWM3 millis

main.cpp@17:3c5df2982199, 2016-12-10 (annotated)

Committer:
benkatz
Date:
Sat Dec 10 05:46:08 2016 +0000
Revision:
17:3c5df2982199
Parent:
15:ef00814e38e2
Child:
18:f1d56f4acb39
Initial CAN Import;

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