Ben Katz / Mbed 2 deprecated Hobbyking_Cheetah_F334

f334

Dependencies:   CANnucleo FastPWM3 mbed

Fork of Hobbyking_Cheetah_Compact by Ben Katz

main.cpp@23:db1263aae417, 2017-11-02 (annotated)

Committer:
benkatz
Date:
Thu Nov 02 22:28:25 2017 +0000
Revision:
23:db1263aae417
Parent:
22:c8a1f2071bb0 
derp;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
benkatz 22:c8a1f2071bb0 1 /*
benkatz 17:3c5df2982199 2 const unsigned int BOARDNUM = 0x2;
benkatz 17:3c5df2982199 3
benkatz 17:3c5df2982199 4 //const unsigned int a_id =
benkatz 17:3c5df2982199 5
benkatz 17:3c5df2982199 6 const unsigned int TX_ID = 0x0100;
benkatz 17:3c5df2982199 7
benkatz 18:f1d56f4acb39 8 const unsigned int cmd_ID = (BOARDNUM<<8) + 0x7;
benkatz 18:f1d56f4acb39 9
benkatz 22:c8a1f2071bb0 10 */
benkatz 17:3c5df2982199 11
benkatz 22:c8a1f2071bb0 12 //#include "CANnucleo.h"
benkatz 0:4e1c4df6aabd 13 #include "mbed.h"
benkatz 0:4e1c4df6aabd 14 #include "PositionSensor.h"
benkatz 20:bf9ea5125d52 15 #include "structs.h"
benkatz 20:bf9ea5125d52 16 #include "foc.h"
benkatz 20:bf9ea5125d52 17 #include "hw_setup.h"
benkatz 20:bf9ea5125d52 18 #include "math_ops.h"
benkatz 20:bf9ea5125d52 19 #include "current_controller_config.h"
benkatz 20:bf9ea5125d52 20 #include "hw_config.h"
benkatz 20:bf9ea5125d52 21 #include "motor_config.h"
benkatz 9:d7eb815cb057 22
benkatz 20:bf9ea5125d52 23 GPIOStruct gpio;
benkatz 20:bf9ea5125d52 24 ControllerStruct controller;
benkatz 20:bf9ea5125d52 25 COMStruct com;
benkatz 17:3c5df2982199 26
benkatz 9:d7eb815cb057 27
benkatz 22:c8a1f2071bb0 28 /*
benkatz 17:3c5df2982199 29 CANnucleo::CAN can(PB_8, PB_9); // CAN Rx pin name, CAN Tx pin name
benkatz 17:3c5df2982199 30 CANnucleo::CANMessage rxMsg;
benkatz 17:3c5df2982199 31 CANnucleo::CANMessage txMsg;
benkatz 22:c8a1f2071bb0 32 */
benkatz 17:3c5df2982199 33 int ledState;
benkatz 17:3c5df2982199 34 int counter = 0;
benkatz 18:f1d56f4acb39 35 int canCmd = 1000;
benkatz 17:3c5df2982199 36 volatile bool msgAvailable = false;
benkatz 17:3c5df2982199 37
benkatz 22:c8a1f2071bb0 38 //DigitalOut toggle(PA_0);
benkatz 20:bf9ea5125d52 39 Ticker loop;
benkatz 17:3c5df2982199 40 /**
benkatz 17:3c5df2982199 41 * @brief 'CAN receive-complete' interrup handler.
benkatz 17:3c5df2982199 42 * @note Called on arrival of new CAN message.
benkatz 17:3c5df2982199 43 * Keep it as short as possible.
benkatz 17:3c5df2982199 44 * @param
benkatz 17:3c5df2982199 45 * @retval
benkatz 17:3c5df2982199 46 */
benkatz 22:c8a1f2071bb0 47 /*
benkatz 17:3c5df2982199 48 void onMsgReceived() {
benkatz 17:3c5df2982199 49 msgAvailable = true;
benkatz 17:3c5df2982199 50 //printf("ping\n\r");
benkatz 17:3c5df2982199 51 }
benkatz 17:3c5df2982199 52
benkatz 17:3c5df2982199 53 void sendCMD(int TX_addr, int val){
benkatz 17:3c5df2982199 54 txMsg.clear(); //clear Tx message storage
benkatz 17:3c5df2982199 55 txMsg.id = TX_addr;
benkatz 17:3c5df2982199 56 txMsg << val;
benkatz 17:3c5df2982199 57 can.write(txMsg);
benkatz 17:3c5df2982199 58 //wait(.1);
benkatz 17:3c5df2982199 59
benkatz 17:3c5df2982199 60 }
benkatz 17:3c5df2982199 61
benkatz 18:f1d56f4acb39 62 void readCAN(void){
benkatz 18:f1d56f4acb39 63 if(msgAvailable) {
benkatz 18:f1d56f4acb39 64 msgAvailable = false; // reset flag for next use
benkatz 18:f1d56f4acb39 65 can.read(rxMsg); // read message into Rx message storage
benkatz 18:f1d56f4acb39 66 // Filtering performed by software:
benkatz 18:f1d56f4acb39 67 if(rxMsg.id == cmd_ID) { // See comments in CAN.cpp for filtering performed by hardware
benkatz 18:f1d56f4acb39 68 rxMsg >> canCmd;
benkatz 18:f1d56f4acb39 69 } // extract first data item
benkatz 18:f1d56f4acb39 70 }
benkatz 18:f1d56f4acb39 71 }
benkatz 17:3c5df2982199 72
benkatz 20:bf9ea5125d52 73 void cancontroller(void){
benkatz 18:f1d56f4acb39 74 //printf("%d\n\r", canCmd);
benkatz 18:f1d56f4acb39 75 readCAN();
benkatz 18:f1d56f4acb39 76 //sendCMD(TX_ID, canCmd);
benkatz 18:f1d56f4acb39 77
benkatz 17:3c5df2982199 78 //sendCMD(TX_ID+b_ID, b1);
benkatz 17:3c5df2982199 79 //sendCMD(TX_ID+c_ID, c1);
benkatz 17:3c5df2982199 80 }
benkatz 17:3c5df2982199 81
benkatz 22:c8a1f2071bb0 82 */
benkatz 20:bf9ea5125d52 83
benkatz 9:d7eb815cb057 84 Serial pc(PA_2, PA_3);
benkatz 8:10ae7bc88d6e 85
benkatz 22:c8a1f2071bb0 86 //PositionSensorAM5147 spi(16384, 0 , NPP); ///1 I really need an eeprom or something to store this....
benkatz 20:bf9ea5125d52 87 //PositionSensorEncoder encoder(4096, 0, 21);
benkatz 20:bf9ea5125d52 88
benkatz 20:bf9ea5125d52 89 int count = 0;
benkatz 20:bf9ea5125d52 90
benkatz 0:4e1c4df6aabd 91
benkatz 1:b8bceb4daed5 92 // Current Sampling IRQ
benkatz 22:c8a1f2071bb0 93 volatile float dtc1 = 0;
benkatz 22:c8a1f2071bb0 94 volatile float dtc2 = 0;
benkatz 22:c8a1f2071bb0 95 volatile float dtc3 = 0;
benkatz 22:c8a1f2071bb0 96 extern "C" void TIM1_UP_TIM16_IRQHandler(void) {
benkatz 2:8724412ad628 97 if (TIM1->SR & TIM_SR_UIF ) {
benkatz 22:c8a1f2071bb0 98
benkatz 22:c8a1f2071bb0 99 GPIOC->ODR ^= (1 << 4);
benkatz 22:c8a1f2071bb0 100
benkatz 22:c8a1f2071bb0 101 ADC1->CR |= ADC_CR_ADSTART;
benkatz 22:c8a1f2071bb0 102 volatile int eoc;
benkatz 22:c8a1f2071bb0 103 while(!eoc){
benkatz 22:c8a1f2071bb0 104 eoc = ADC1->ISR & ADC_ISR_EOC;
benkatz 22:c8a1f2071bb0 105 }
benkatz 22:c8a1f2071bb0 106
benkatz 22:c8a1f2071bb0 107 controller.adc1_raw = ADC1->DR;
benkatz 22:c8a1f2071bb0 108 controller.adc2_raw = ADC2->DR;
benkatz 20:bf9ea5125d52 109
benkatz 22:c8a1f2071bb0 110 //spi.Sample();
benkatz 22:c8a1f2071bb0 111 //controller.dtheta_mech = spi.GetMechVelocity();
benkatz 22:c8a1f2071bb0 112 //controller.theta_elec = spi.GetElecPosition();
benkatz 22:c8a1f2071bb0 113
benkatz 22:c8a1f2071bb0 114 commutate(&controller, &gpio, controller.theta_elec);
benkatz 22:c8a1f2071bb0 115 //TIM1->CCR1 = 0x708*(dtc3);
benkatz 22:c8a1f2071bb0 116 //TIM1->CCR2 = 0x708*(dtc2);
benkatz 22:c8a1f2071bb0 117 //TIM1->CCR3 = 0x708*(dtc1);
benkatz 22:c8a1f2071bb0 118 GPIOC->ODR ^= (1 << 4);
benkatz 22:c8a1f2071bb0 119
benkatz 22:c8a1f2071bb0 120 //gpio.pwm_u->write(1.0f); //write duty cycles
benkatz 22:c8a1f2071bb0 121
benkatz 22:c8a1f2071bb0 122
benkatz 20:bf9ea5125d52 123
benkatz 2:8724412ad628 124 }
benkatz 2:8724412ad628 125 TIM1->SR = 0x0; // reset the status register
benkatz 2:8724412ad628 126 }
benkatz 0:4e1c4df6aabd 127
benkatz 20:bf9ea5125d52 128
benkatz 22:c8a1f2071bb0 129 void enter_torque_mode(void){
benkatz 22:c8a1f2071bb0 130
benkatz 22:c8a1f2071bb0 131 TIM1->CR1 ^= TIM_CR1_UDIS; //enable interrupt
benkatz 22:c8a1f2071bb0 132 controller.i_d_ref = 0;
benkatz 22:c8a1f2071bb0 133 controller.i_q_ref = 0;
benkatz 22:c8a1f2071bb0 134 reset_foc(&controller);
benkatz 22:c8a1f2071bb0 135 gpio.enable->write(1);
benkatz 22:c8a1f2071bb0 136 }
benkatz 22:c8a1f2071bb0 137
benkatz 0:4e1c4df6aabd 138
benkatz 0:4e1c4df6aabd 139 int main() {
benkatz 20:bf9ea5125d52 140
benkatz 22:c8a1f2071bb0 141
benkatz 20:bf9ea5125d52 142 controller.v_bus = V_BUS;
benkatz 22:c8a1f2071bb0 143 //spi.ZeroPosition();
benkatz 20:bf9ea5125d52 144 Init_All_HW(&gpio);
benkatz 20:bf9ea5125d52 145
benkatz 9:d7eb815cb057 146 wait(.1);
benkatz 20:bf9ea5125d52 147 //TIM1->CR1 |= TIM_CR1_UDIS;
benkatz 20:bf9ea5125d52 148 gpio.enable->write(1);
benkatz 20:bf9ea5125d52 149 gpio.pwm_u->write(1.0f); //write duty cycles
benkatz 20:bf9ea5125d52 150 gpio.pwm_v->write(1.0f);
benkatz 20:bf9ea5125d52 151 gpio.pwm_w->write(1.0f);
benkatz 20:bf9ea5125d52 152 zero_current(&controller.adc1_offset, &controller.adc2_offset);
benkatz 22:c8a1f2071bb0 153 //gpio.enable->write(0);
benkatz 20:bf9ea5125d52 154 reset_foc(&controller);
benkatz 22:c8a1f2071bb0 155
benkatz 22:c8a1f2071bb0 156 //TIM1->CR1 ^= TIM_CR1_UDIS; //enable interrupt
benkatz 20:bf9ea5125d52 157 gpio.enable->write(1);
benkatz 22:c8a1f2071bb0 158 gpio.pwm_u->write(1.0f - .05f); //write duty cycles
benkatz 22:c8a1f2071bb0 159 gpio.pwm_v->write(1.0f - .05f);
benkatz 22:c8a1f2071bb0 160 gpio.pwm_w->write(1.0f - .1f);
benkatz 20:bf9ea5125d52 161
benkatz 20:bf9ea5125d52 162 wait(.1);
benkatz 20:bf9ea5125d52 163 //loop.attach(&commutate, .000025);
benkatz 22:c8a1f2071bb0 164 //can.frequency(1000000); // set bit rate to 1Mbps
benkatz 22:c8a1f2071bb0 165 //can.attach(&onMsgReceived); // attach 'CAN receive-complete' interrupt handler
benkatz 22:c8a1f2071bb0 166 //can.filter(0x020 << 25, 0xF0000004, CANAny, 0);
benkatz 18:f1d56f4acb39 167
benkatz 14:80ce59119d93 168 pc.baud(921600);
benkatz 20:bf9ea5125d52 169 wait(.01);
benkatz 20:bf9ea5125d52 170 pc.printf("HobbyKing Cheetah v1.1\n\r");
benkatz 20:bf9ea5125d52 171 pc.printf("ADC1 Offset: %d ADC2 Offset: %d", controller.adc1_offset, controller.adc2_offset);
benkatz 20:bf9ea5125d52 172 wait(.01);
benkatz 20:bf9ea5125d52 173
benkatz 22:c8a1f2071bb0 174
benkatz 22:c8a1f2071bb0 175 enter_torque_mode();
benkatz 22:c8a1f2071bb0 176 //enter_zeroing_mode();
benkatz 22:c8a1f2071bb0 177
benkatz 22:c8a1f2071bb0 178 GPIOC->ODR = 0;
benkatz 22:c8a1f2071bb0 179
benkatz 0:4e1c4df6aabd 180 while(1) {
benkatz 11:c83b18d41e54 181
benkatz 0:4e1c4df6aabd 182 }
benkatz 0:4e1c4df6aabd 183 }