[Ver 1.0] The code was given by Seunghoon shin, used for hydraulic quadrupedal robot. Buyoun Cho will revise the code for Post-LIGHT (the robot name is not determined yet).

Dependencies:   mbed FastPWM

main.cpp

Committer:
jobuuu
Date:
2019-08-20
Revision:
2:a1c0a37df760
Parent:
1:e04e563be5ce
Child:
5:a4319f79457b

File content as of revision 2:a1c0a37df760:

#include "mbed.h"
#include "FastPWM.h"
#include "INIT_HW.h"
#include "function_CAN.h"
#include "SPI_EEP_ENC.h"
#include "I2C_AS5510.h"
#include "setting.h"

// dac & check
DigitalOut check(PC_2);
DigitalOut check_2(PC_3);
AnalogOut dac_1(PA_4);
AnalogOut dac_2(PA_5);
AnalogIn adc3(PC_1);

// pwm
double dtc_v=0.0;
double dtc_w=0.0;

// I2C
I2C i2c(PC_9,PA_8); // SDA, SCL (for K22F)
const int i2c_slave_addr1 =  0x56;
unsigned int value; // 10bit output of reading sensor AS5510

// SPI
SPI eeprom(PB_15, PB_14, PB_13); // EEPROM //(SPI_MOSI, SPI_MISO, SPI_SCK);
DigitalOut eeprom_cs(PB_12);
SPI enc(PC_12,PC_11,PC_10);
DigitalOut enc_cs(PD_2);
DigitalOut indi_led(PA_15);

// UART
Serial pc(PA_9,PA_10); //  _ UART

//CAN
CAN can(PB_8, PB_9, 1000000);
CANMessage msg;

// Variables


double a1;
double PWM_out;
double temp_cur=0.0;
int main()
{
    /*******************************************************
    ***     Initialization
    ********************************************************/
        
    indi_led = 0;
    pc.baud(9600);
    
    //Timer t;
    //t.start();
    //t.stop();
    //pc.printf("The time taken was %f seconds\n",t.read());

    // i2c init
//    i2c.frequency(400 * 1000);          // 0.4 mHz
//    wait_ms(2);                         // Power Up wait
//    look_for_hardware_i2c();            // Hardware present
//    init_as5510(i2c_slave_addr1);

//    // spi init
    eeprom.format(8,3);
    eeprom.frequency(5000000); //5M
    enc.format(8,0);
    enc.frequency(5000000); //5M
    
    // ADC init
//    Init_ADC();

    // Pwm init
    Init_PWM();
    TIM4->CR1 ^= TIM_CR1_UDIS;
    
//    //SPI
//    spi_eeprom_ready();
//    spi_eeprom_write(0x1,0x112);
//    spi_eeprom_ready();
//    int i = spi_eeprom_read(0x1);

    // CAN
    can.attach(&CAN_RX_HANDLER);
    
    // spi _ enc
    spi_enc_set_init();
    
    
    /*******************************************************
    ***     Program is operating!
    ********************************************************/
    while(1) {
//        dac_1=0.5;
//        dac_2=0.1;
        check_2 = 1;
        //spi _ eeprom
//        spi_eeprom_ready();
//        spi_eeprom_write(0x0001,0xFFFFFFFF);
//        spi_eeprom_ready();
//        int a=spi_eeprom_read(0x0001);
        //spi _ enc
        int a = spi_enc_read();
//        read_field(i2c_slave_addr1);

        check_2=0;

//        pc.printf("%f\n",PWM_out);
//        pc.printf("%d\n",a1);
        
//        wait(0.01f);
    }
}

/*******************************************************************************
                            TIMER INTERRUPT
*******************************************************************************/

unsigned long CNT_TMR4 = 0;
double FREQ_TMR4 = (double)FREQ_20k;
extern "C" void TIM4_IRQHandler(void)
{
    if ( TIM4->SR & TIM_SR_UIF ) {
        /*******************************************************
        ***     Sensor Read & Data Handling 
        ********************************************************/
        
        if((CNT_TMR4%2)==0){
            //spi
    //      eeprom.write(0xff);
    //      eeprom.write(0xff);
    //      ready();
    //      read(1);
    
            //i2c
    ////    read_field(i2c_slave_addr1);
    
            //ADC
    //        ADC1->CR2  |= 0x40000000;                        // adc _ 12bit
    //      a1=ADC1->DR;
    //      a1=ADC2->DR;
    //        int raw_cur = ADC3->DR;
            double raw_cur=(double)adc3*4096.0;
        
            double alpha_update_cur = 1.0/(1.0+(FREQ_TMR4/2.0)/(2.0*3.14*100.0)); // f_cutoff : 500Hz
            double new_a1 = (double)raw_cur;
            a1=a1*(1.0-alpha_update_cur)+new_a1*(alpha_update_cur);
//            a1=new_a1;
        }

        //DAC
//      dac_1 = ADC1->DR;
//      dac_2 = ADC2->DR;
        
        /*******************************************************
        ***     Valve Control 
        ********************************************************/
        
        // PWM_out : -1.0~1.0(full duty)
        PWM_out = -0.0104;
        double t = (double)CNT_TMR4/FREQ_TMR4;
        double T = 10.0; 
//        PWM_out = 0.36*sin(2.0*PI*t/T);
        PWM_out = 0.04*sin(2.0*PI*t/T);
//        if(PWM_out > 0.0) PWM_out = 0.02;
//        else if(PWM_out < 0.0) PWM_out = -0.02; 
        
        // Saturation of output voltage to 5.0V
        if(PWM_out > (5.0/12.0)) PWM_out=(5.0/12.0);
        else if (PWM_out < (-5.0/12.0)) PWM_out=(-5.0/12.0);
        
        if (PWM_out>0.0) {
            dtc_v=0.0;
            dtc_w=PWM_out;
        } else {
            dtc_v=-PWM_out;
            dtc_w=0.0;
        }
        
        //pwm
        TIM4->CCR2 = (PWM_ARR)*(1.0-dtc_v);
        TIM4->CCR1 = (PWM_ARR)*(1.0-dtc_w);
        
        /*******************************************************
        ***     Data Send (CAN) & Print out (UART)
        ********************************************************/
        if((CNT_TMR4%40)==0){
            msg.id = 50;
            msg.len = 4;
            int temp_CUR = (int)(a1);
            msg.data[0]=0x00FF&temp_CUR;
            msg.data[1]=0x00FF&(temp_CUR>>8);
            int temp_PWM = (int)(PWM_out*10000.0);
            msg.data[2]=0x00FF&temp_PWM;
            msg.data[3]=0x00FF&(temp_PWM>>8);
            can.write(msg);
        }

//        if((CNT_TMR4%4000)==0){
//            pc.printf("%d\n",a1);
//        }

        /*******************************************************
        ***     Timer Counting & etc.
        ********************************************************/
        CNT_TMR4++; 
    }
    TIM4->SR = 0x0;  // reset the status register
}