main.cpp

00001 /*
00002 const unsigned int BOARDNUM = 0x2;
00003 
00004 //const unsigned int a_id =                            
00005 
00006 const unsigned int TX_ID = 0x0100;
00007     
00008 const unsigned int cmd_ID = (BOARDNUM<<8) + 0x7;
00009 
00010 */
00011 
00012 //#include "CANnucleo.h"
00013 #include "mbed.h"
00014 #include "PositionSensor.h"
00015 #include "structs.h"
00016 #include "foc.h"
00017 #include "hw_setup.h"
00018 #include "math_ops.h"
00019 #include "current_controller_config.h"
00020 #include "hw_config.h"
00021 #include "motor_config.h"
00022 
00023 GPIOStruct gpio;
00024 ControllerStruct controller;
00025 COMStruct com;
00026 
00027 
00028 /*
00029 CANnucleo::CAN          can(PB_8, PB_9);  // CAN Rx pin name, CAN Tx pin name
00030 CANnucleo::CANMessage   rxMsg;
00031 CANnucleo::CANMessage   txMsg;
00032 */
00033 int                     ledState;
00034 int                     counter = 0;
00035 int canCmd = 1000;
00036 volatile bool           msgAvailable = false;
00037 
00038 //DigitalOut toggle(PA_0);
00039 Ticker  loop;
00040 /**
00041  * @brief   'CAN receive-complete' interrup handler.
00042  * @note    Called on arrival of new CAN message.
00043  *          Keep it as short as possible.
00044  * @param   
00045  * @retval  
00046  */
00047  /*
00048 void onMsgReceived() {
00049     msgAvailable = true;
00050     //printf("ping\n\r");
00051 }
00052 
00053 void sendCMD(int TX_addr, int val){
00054     txMsg.clear();      //clear Tx message storage
00055     txMsg.id = TX_addr;
00056     txMsg << val;
00057     can.write(txMsg);
00058     //wait(.1);
00059     
00060     }
00061     
00062 void readCAN(void){
00063     if(msgAvailable) { 
00064     msgAvailable = false;               // reset flag for next use
00065     can.read(rxMsg);                    // read message into Rx message storage
00066     // Filtering performed by software:           
00067     if(rxMsg.id == cmd_ID) {             // See comments in CAN.cpp for filtering performed by hardware
00068             rxMsg >> canCmd;  
00069             }             // extract first data item
00070         }
00071         }
00072     
00073 void cancontroller(void){
00074     //printf("%d\n\r", canCmd);
00075     readCAN();
00076     //sendCMD(TX_ID, canCmd);
00077     
00078     //sendCMD(TX_ID+b_ID, b1);
00079     //sendCMD(TX_ID+c_ID, c1);
00080     }
00081     
00082 */
00083 
00084 Serial pc(PA_2, PA_3);
00085 
00086 //PositionSensorAM5147 spi(16384, 0 , NPP);   ///1  I really need an eeprom or something to store this....
00087 //PositionSensorEncoder encoder(4096, 0, 21); 
00088 
00089 int count = 0;
00090        
00091 
00092 // Current Sampling IRQ
00093 volatile float dtc1 = 0;
00094 volatile float dtc2 = 0;
00095 volatile float dtc3 = 0;
00096 extern "C" void TIM1_UP_TIM16_IRQHandler(void) {
00097   if (TIM1->SR & TIM_SR_UIF ) {
00098         
00099         GPIOC->ODR ^= (1 << 4);
00100         
00101         ADC1->CR  |= ADC_CR_ADSTART;  
00102         volatile int eoc;
00103         while(!eoc){
00104             eoc = ADC1->ISR & ADC_ISR_EOC;
00105             }
00106         
00107         controller.adc1_raw = ADC1->DR;
00108         controller.adc2_raw = ADC2->DR;
00109         
00110         //spi.Sample();
00111         //controller.dtheta_mech = spi.GetMechVelocity();
00112         //controller.theta_elec = spi.GetElecPosition();
00113         
00114         commutate(&controller, &gpio, controller.theta_elec);   
00115         //TIM1->CCR1 = 0x708*(dtc3);
00116         //TIM1->CCR2 = 0x708*(dtc2);
00117         //TIM1->CCR3 = 0x708*(dtc1);
00118         GPIOC->ODR ^= (1 << 4);
00119         
00120         //gpio.pwm_u->write(1.0f);  //write duty cycles
00121         
00122 
00123          
00124       }
00125   TIM1->SR = 0x0; // reset the status register
00126 }
00127 
00128        
00129 void enter_torque_mode(void){
00130     
00131     TIM1->CR1 ^= TIM_CR1_UDIS; //enable interrupt
00132     controller.i_d_ref = 0;
00133     controller.i_q_ref = 0;
00134     reset_foc(&controller);
00135     gpio.enable->write(1);
00136     }
00137     
00138        
00139 int main() {
00140 
00141 
00142     controller.v_bus = V_BUS;
00143     //spi.ZeroPosition();
00144     Init_All_HW(&gpio);
00145 
00146     wait(.1);
00147     //TIM1->CR1 |= TIM_CR1_UDIS;
00148     gpio.enable->write(1);
00149     gpio.pwm_u->write(1.0f);  //write duty cycles
00150     gpio.pwm_v->write(1.0f);
00151     gpio.pwm_w->write(1.0f);
00152     zero_current(&controller.adc1_offset, &controller.adc2_offset);
00153     //gpio.enable->write(0);
00154     reset_foc(&controller);
00155     
00156     //TIM1->CR1 ^= TIM_CR1_UDIS; //enable interrupt
00157     gpio.enable->write(1);
00158     gpio.pwm_u->write(1.0f - .05f);  //write duty cycles
00159     gpio.pwm_v->write(1.0f - .05f);
00160     gpio.pwm_w->write(1.0f - .1f);
00161     
00162     wait(.1);
00163     //loop.attach(&commutate, .000025);
00164     //can.frequency(1000000);                     // set bit rate to 1Mbps
00165     //can.attach(&onMsgReceived);                 // attach 'CAN receive-complete' interrupt handler
00166     //can.filter(0x020 << 25, 0xF0000004, CANAny, 0);
00167     
00168     pc.baud(921600);
00169     wait(.01);
00170     pc.printf("HobbyKing Cheetah v1.1\n\r");
00171     pc.printf("ADC1 Offset: %d    ADC2 Offset: %d", controller.adc1_offset, controller.adc2_offset);
00172     wait(.01);
00173     
00174     
00175     enter_torque_mode();
00176     //enter_zeroing_mode();
00177     
00178     GPIOC->ODR = 0;
00179     
00180     while(1) {
00181 
00182     }
00183 }