rainbow

Dependencies:   mbed FastPWM

Files at this revision

API Documentation at this revision

Comitter:
Lightvalve
Date:
Mon Jun 13 08:48:55 2022 +0000
Parent:
242:b235d67d25ba
Child:
244:e9c5ec04e378
Commit message:
220613

Changed in this revision

CAN/function_CAN.cpp Show annotated file Show diff for this revision Revisions of this file
CAN/function_CAN.h Show annotated file Show diff for this revision Revisions of this file
I2C_AS5510/I2C_AS5510.cpp Show diff for this revision Revisions of this file
I2C_AS5510/I2C_AS5510.h Show diff for this revision Revisions of this file
INIT_HW/INIT_HW.cpp Show annotated file Show diff for this revision Revisions of this file
INIT_HW/INIT_HW.h Show annotated file Show diff for this revision Revisions of this file
function_utilities/function_utilities.cpp Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
setting.h Show annotated file Show diff for this revision Revisions of this file
--- a/CAN/function_CAN.cpp	Thu Apr 07 06:18:38 2022 +0000
+++ b/CAN/function_CAN.cpp	Mon Jun 13 08:48:55 2022 +0000
@@ -675,8 +675,8 @@
 void CAN_RX_HANDLER()
 {
     
-    if (LED > 0) LED = 0;
-    else LED = 1;
+//    if (LED > 0) LED = 0;
+//    else LED = 1;
     
     can.read(msg);
     
@@ -703,7 +703,7 @@
 
         if(SUPPLY_PRESSURE_UPDATE == 1) {
             int16_t temp_REF_Ps = (int16_t) (msg.data[6] | msg.data[7] << 8);
-            PRES_SUPPLY = ((float)temp_REF_Ps) / 100.0; 
+            PRES_SUPPLY = ((float)temp_REF_Ps) / 100.0f; 
             if(PRES_SUPPLY<35.0f) PRES_SUPPLY = 35.0f;
             else if(PRES_SUPPLY>210.0f) PRES_SUPPLY = 210.0f;
         } else {
--- a/CAN/function_CAN.h	Thu Apr 07 06:18:38 2022 +0000
+++ b/CAN/function_CAN.h	Mon Jun 13 08:48:55 2022 +0000
@@ -242,11 +242,6 @@
 extern State valve_pos;
 extern State valve_pos_raw;
 
-extern State INIT_Vout;
-extern State INIT_Valve_Pos;
-extern State INIT_Pos;
-extern State INIT_torq;
-
 // CAN Receive Functions
 void ReadCMD();
 void CAN_RX_HANDLER();
--- a/I2C_AS5510/I2C_AS5510.cpp	Thu Apr 07 06:18:38 2022 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,100 +0,0 @@
-#include "mbed.h"
-#include "setting.h"
-
-void look_for_hardware_i2c()
-{
-    //pc.printf("\r\n\n\n");
-    //pc.printf("Note I2C address 0x1C used by FXOS8700CQ 3-axis accelerometer and 3-axis magetometer\r\n");
-    //pc.printf("Start hardware search..... \r\n");
-
-    int count = 0;
-    for (int address=12; address<256; address+=2) {
-        if (!i2c.write(address, NULL, 0)) {         // 0 returned is OK
-            //pc.printf(" - I2C device found at address 0x%02X\n\r", address >>1);
-            count++;
-        }
-    }
-    //pc.printf("%d devices found \n\r", count);
-}
-
-void init_as5510(int i2c_address)
-{
-    int i2c_adrs=0;
-    char idata[2];
-    int result=0;
-
-    //pc.printf("\r\n");
-    //pc.printf("Start AS5510 init.. \r\n");
-
-    i2c_adrs= (i2c_address << 1);                   // AS5510 Slave address lsb= 0 for write
-
-    //---------- Magnet selection --------------------------------
-    //----0x00= <50mT-----------Strong magnet
-    //----0x01= <25mT
-    //----0x02= <18.7mT
-    //----0x03= <12.5mT---------Weak magnet
-    //-----------------------------------------------------------
-    idata[0]=0x0B;                                  // Register for Sensitivity
-    idata[1]=0x00;                                  // Byte
-    result= i2c.write(i2c_adrs, idata, 2, 0);       // Now write_sensitivity
-//    if (result != 0) pc.printf("No ACK bit! (09)\n\r");
-
-    //----------- Operation mode selection------------------------
-    idata[0]=0x02;                                  // 0x02 address setup register for operation, speed, polarity
-    idata[1]=0x04;                                  // Normal Operation, Slow mode (1), NORMAL Polarity (0), Power Up (0)
-    result= i2c.write(i2c_adrs, idata, 2, 0);       // Now write_operation
-//    if (result != 0) pc.printf("No ACK bit! (11)\n\r");
-
-    //pc.printf("AS5510 init done\r\n");
-}
-
-
-int offset_comp(int i2c_address)
-{
-    int adrss=0;
-    int oresult=0;
-    char off_data[2];
-    int ocf_done=0;
-
-    // First, now Write pointer to register 0x00----------------------------
-    adrss= (i2c_address << 1);                  // AS5510 Slave address lsb= 0 for write
-    oresult= i2c.write(adrss, 0x00, 1, 0);      // write one byte
-    if (oresult != 0) //pc.printf("No ACK bit! (33)\n\r");
-
-    // Second, now Read register 0x00 and 0x01--------------------------------
-    memset(off_data, 0, sizeof(off_data));
-    adrss= (i2c_address << 1) | 0x01;           // AS5510 address lsb= 1 for read
-    oresult= i2c.read(adrss, off_data, 2, 0);   // read two bytes
-
-    // Now analyse register 0x01 ----------------------------------------------
-    ocf_done= off_data[1] & 0x08;               // mask off bits, 1= done
-    if (ocf_done== 0)  return(0);               // return(0)= compensation process is pending
-    else return(1);                             // return(1)= compensation process is completed
-}
-
-
-void read_field(int i2c_address)
-{
-    int adr=0;
-    char rx_data[2];
-    int rresult=0;
-    char lsb, msb;
-
-    // First, now Write pointer to register 0x00----------------------------
-    adr= (i2c_address << 1);                        // AS5510 address lsb= 0 for write
-    rresult= i2c.write(adr, 0x00, 1, 0);            // write one byte to register 0x00 for magnetic field strength
-//    if (rresult != 0) pc.printf("No ACK bit! (22)\n\r");
-
-    // Second, now Read register 0x00 and 0x01--------------------------------
-    memset(rx_data, 0, sizeof(rx_data));
-    adr= (i2c_address << 1) | 0x01;                 // AS5510 address lsb= 1 for read
-    rresult= i2c.read(adr, rx_data, 2, 0);          // read two bytes
-
-
-    // Now analyse register 0x01 ----------------------------------------------
-    lsb= rx_data[0];                                // get LSB
-    msb= rx_data[1]&0x03;                           // need only 2 low bits og MSB
-    value = ((msb & 0x03)<<8) + lsb;
-//    pc.printf("I2C adres= 0x%02X, Magnetic Field => msb= 0x%02X, lsb= 0x%02X, decimal 10-bit value = %u \r\n ", i2c_address, rx_data[0],rx_data[1], value);
-
-}
\ No newline at end of file
--- a/I2C_AS5510/I2C_AS5510.h	Thu Apr 07 06:18:38 2022 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,12 +0,0 @@
-#ifndef _I2C_AS5510_H_
-#define _I2C_AS5510_H_
-
-#include "mbed.h"
-#include "setting.h"
-
-void look_for_hardware_i2c();
-void init_as5510(int i2c_address);
-int offset_comp(int i2c_address);
-void read_field(int i2c_address);
-
-#endif
--- a/INIT_HW/INIT_HW.cpp	Thu Apr 07 06:18:38 2022 +0000
+++ b/INIT_HW/INIT_HW.cpp	Mon Jun 13 08:48:55 2022 +0000
@@ -2,92 +2,158 @@
 #include "FastPWM.h"
 #include "setting.h"
 
-void Init_ADC(void){
+extern ADC_HandleTypeDef        hadc1;
+extern ADC_HandleTypeDef        hadc2;
+
+//void Init_ADC(void){
+//    // ADC Setup
+//     RCC->APB2ENR |= RCC_APB2ENR_ADC3EN;                        // clock for ADC3
+//     RCC->APB2ENR |= RCC_APB2ENR_ADC2EN;                        // clock for ADC2
+//     RCC->APB2ENR |= RCC_APB2ENR_ADC1EN;                        // clock for ADC1
+//
+//     RCC->AHB1ENR |= RCC_AHB1ENR_GPIOCEN;                        // Enable clock for GPIOC
+//     RCC->AHB1ENR |= RCC_AHB1ENR_GPIOBEN;                        // Enable clock for GPIOA
+//
+//     ADC->CCR = 0x00000016;                                     // Regular simultaneous mode only
+//     ADC1->CR2 |= ADC_CR2_ADON;//0x00000001;                    // ADC1 ON
+//     ADC1->SQR3 = 0x0000000E;                    //channel      // use PC_4 as input- ADC1_IN14
+//     ADC2->CR2 |= ADC_CR2_ADON;//0x00000001;                    // ADC2 ON
+//     ADC2->SQR3 = 0x00000008;                                   // use PB_0 as input - ADC2_IN8
+//     ADC3->CR2 |= ADC_CR2_ADON;                                 // ADC3 ON
+//     ADC3->SQR3 = 0x0000000B;                                   // use PC_1, - ADC3_IN11
+//     GPIOC->MODER |= 0b1100001100;             //each channel   // PC_4, PC_1 are analog inputs
+//     GPIOB->MODER |= 0x3;                                       // PB_0 as analog input
+//
+//     ADC1->SMPR1 |= 0x00001000;                                     // 15 cycles on CH_14, 0b0001000000000000
+//     ADC2->SMPR2 |= 0x01000000;                                     // 15 cycles on CH_8,  0b0000000100000000<<16
+//     ADC3->SMPR1 |= 0x00000008;                                     // 15 cycles on CH_11, 0b0000000000001000
+//}
+
+
+void Init_ADC(void)
+{
     // ADC Setup
-     RCC->APB2ENR |= RCC_APB2ENR_ADC3EN;                        // clock for ADC3
-     RCC->APB2ENR |= RCC_APB2ENR_ADC2EN;                        // clock for ADC2
-     RCC->APB2ENR |= RCC_APB2ENR_ADC1EN;                        // clock for ADC1
-     
-     RCC->AHB1ENR |= RCC_AHB1ENR_GPIOCEN;                        // Enable clock for GPIOC
-     RCC->AHB1ENR |= RCC_AHB1ENR_GPIOBEN;                        // Enable clock for GPIOA
-    
-     ADC->CCR = 0x00000016;                                     // Regular simultaneous mode only
-     ADC1->CR2 |= ADC_CR2_ADON;//0x00000001;                    // ADC1 ON
-     ADC1->SQR3 = 0x0000000E;                    //channel      // use PC_4 as input- ADC1_IN14
-     ADC2->CR2 |= ADC_CR2_ADON;//0x00000001;                    // ADC2 ON
-     ADC2->SQR3 = 0x00000008;                                   // use PB_0 as input - ADC2_IN8
-     ADC3->CR2 |= ADC_CR2_ADON;                                 // ADC3 ON
-     ADC3->SQR3 = 0x0000000B;                                   // use PC_1, - ADC3_IN11
-     GPIOC->MODER |= 0b1100001100;             //each channel   // PC_4, PC_1 are analog inputs 
-     GPIOB->MODER |= 0x3;                                       // PB_0 as analog input
-     
-     ADC1->SMPR1 |= 0x00001000;                                     // 15 cycles on CH_14, 0b0001000000000000
-     ADC2->SMPR2 |= 0x01000000;                                     // 15 cycles on CH_8,  0b0000000100000000<<16
-     ADC3->SMPR1 |= 0x00000008;                                     // 15 cycles on CH_11, 0b0000000000001000
+//     RCC->APB2ENR |= RCC_APB2ENR_ADC3EN;                        // clock for ADC3
+//     RCC->APB2ENR |= RCC_APB2ENR_ADC2EN;                        // clock for ADC2
+    RCC->APB2ENR |= RCC_APB2ENR_ADC1EN;                        // clock for ADC1
+
+//     RCC->AHB1ENR |= RCC_AHB1ENR_GPIOCEN;                        // Enable clock for GPIOC
+    RCC->AHB1ENR |= RCC_AHB1ENR_GPIOBEN;                        // Enable clock for GPIOB
 
-    }
-    
-void Init_PWM(){
+    ADC->CCR = 0x00000016;                                     // Regular simultaneous mode only
+    ADC1->CR2 |= ADC_CR2_ADON;//0x00000001;                    // ADC1 ON
+    ADC1->SQR3 = 0x00000008;                                   // use PB_0 as input - ADC1_IN8
+//     ADC1->SQR3 = 0x0000000E;                    //channel      // use PC_4 as input- ADC1_IN14
+//     ADC2->CR2 |= ADC_CR2_ADON;//0x00000001;                    // ADC2 ON
+//     ADC2->SQR3 = 0x00000008;                                   // use PB_0 as input - ADC2_IN8
+//     ADC3->CR2 |= ADC_CR2_ADON;                                 // ADC3 ON
+//     ADC3->SQR3 = 0x0000000B;                                   // use PC_1, - ADC3_IN11
+//     GPIOC->MODER |= 0b1100001100;             //each channel   // PC_4, PC_1 are analog inputs
+    GPIOB->MODER |= 0x3;                                       // PB_0 as analog input
+
+    ADC1->SMPR2 |= 0x01000000;                                     // 15 cycles on CH_8,  0b0000000100000000<<16
+//     ADC1->SMPR1 |= 0x00001000;                                     // 15 cycles on CH_14, 0b0001000000000000
+//     ADC2->SMPR2 |= 0x01000000;                                     // 15 cycles on CH_8,  0b0000000100000000<<16
+//     ADC3->SMPR1 |= 0x00000008;                                     // 15 cycles on CH_11, 0b0000000000001000
+}
+
+
+
+
+
+void Init_PWM()
+{
 
     RCC->APB1ENR |= RCC_APB1ENR_TIM4EN;                         // enable TIM4 clock
-    FastPWM pwm_v(PIN_V);
-    FastPWM pwm_w(PIN_W);
-    
-     //ISR Setup     
-    
+//    FastPWM pwm_v(PIN_V);
+//    FastPWM pwm_w(PIN_W);
+
+    //ISR Setup
+
     NVIC_EnableIRQ(TIM4_IRQn);                         //Enable TIM4 IRQ
 
     TIM4->DIER |= TIM_DIER_UIE;                                 // enable update interrupt
-//    TIM4->CR1 = 0x40;                                           // CMS = 10, interrupt only when counting up // Center-aligned mode
-    TIM4->CR1 = 0x10;
+    TIM4->CR1 = 0x40;                                           // CMS = 10, interrupt only when counting up // Center-aligned mode
+//    TIM4->CR1 = 0x10;
     TIM4->CR1 |= TIM_CR1_UDIS;
-    TIM4->CR1 |= TIM_CR1_ARPE;                                  // autoreload on, 
-    TIM4->RCR |= 0x001;                                         // update event once per up/down count of TIM4 
+//    TIM4->CR1 |= TIM_CR1_ARPE;                                  // autoreload on,
+    TIM4->RCR |= 0x001;                                         // update event once per up/down count of TIM4
     TIM4->EGR |= TIM_EGR_UG;
- 
+
     //PWM Setup
-
     TIM4->PSC = 0x0;                                            // no prescaler, timer counts up in sync with the peripheral clock
+//    TIM4->PSC = 10-1;                                            // no prescaler, timer counts up in sync with the peripheral clock
     TIM4->ARR = PWM_ARR-1;                                          // set auto reload
     TIM4->CCER |= ~(TIM_CCER_CC1NP);                            // Interupt when low side is on.
     TIM4->CR1 |= TIM_CR1_CEN;                                   // enable TIM4
-    
+
+    TIM4->CCMR1 |= 0x7060;
+
 }
 
 
-void Init_TMR3(){
+//void Init_TMR3()
+//{
+//    RCC->APB1ENR |= RCC_APB1ENR_TIM3EN;                         // enable TIM3 clock
+////    FastPWM pwm_v(PIN_V);
+////    FastPWM pwm_w(PIN_W);
+//    //ISR Setup
+//
+//    NVIC_EnableIRQ(TIM3_IRQn);                                  //Enable TIM3 IRQ
+//
+//    TIM3->DIER |= TIM_DIER_UIE;                                 // enable update interrupt
+////    TIM3->CR1 = 0x40;                                           // CMS = 10, interrupt only when counting up // Center-aligned mode
+//    TIM3->CR1 = 0x10;
+//    TIM3->CR1 |= TIM_CR1_UDIS;
+////    TIM3->CR1 |= TIM_CR1_ARPE;                                  // autoreload on,
+//    TIM3->RCR |= 0x001;                                         // update event once per up/down count of TIM3
+//    TIM3->EGR |= TIM_EGR_UG;
+//
+////    TIM3->PSC = 0x00;                                            // no prescaler, timer counts up in sync with the peripheral clock
+//    TIM3->PSC = 0x01;
+//    TIM3->ARR = TMR3_COUNT-1;                                          // set auto reload, 5 khz
+//    TIM3->CCER |= ~(TIM_CCER_CC1NP);                            // Interupt when low side is on.
+//    TIM3->CR1 |= TIM_CR1_CEN;                                   // enable TIM4
+//}
+
+void Init_TMR3()
+{
     RCC->APB1ENR |= RCC_APB1ENR_TIM3EN;                         // enable TIM3 clock
-    
-     //ISR Setup     
-    
+//    FastPWM pwm_v(PIN_V);
+//    FastPWM pwm_w(PIN_W);
+    //ISR Setup
+
     NVIC_EnableIRQ(TIM3_IRQn);                                  //Enable TIM3 IRQ
 
     TIM3->DIER |= TIM_DIER_UIE;                                 // enable update interrupt
-//    TIM3->CR1 = 0x40;                                           // CMS = 10, interrupt only when counting up // Center-aligned mode
-    TIM3->CR1 = 0x10;
+    TIM3->CR1 = 0x40;                                           // CMS = 10, interrupt only when counting up // Center-aligned mode
+//    TIM3->CR1 = 0x10;
     TIM3->CR1 |= TIM_CR1_UDIS;
-    TIM3->CR1 |= TIM_CR1_ARPE;                                  // autoreload on, 
-    TIM3->RCR |= 0x001;                                         // update event once per up/down count of TIM3 
+//    TIM3->CR1 |= TIM_CR1_ARPE;                                  // autoreload on,
+    TIM3->RCR |= 0x001;                                         // update event once per up/down count of TIM3
     TIM3->EGR |= TIM_EGR_UG;
 
-//    TIM3->PSC = 0x00;                                            // no prescaler, timer counts up in sync with the peripheral clock
-    TIM3->PSC = 0x01;
+    TIM3->PSC = 0x00;                                            // no prescaler, timer counts up in sync with the peripheral clock
+//    TIM3->PSC = 0x01;
     TIM3->ARR = TMR3_COUNT-1;                                          // set auto reload, 5 khz
     TIM3->CCER |= ~(TIM_CCER_CC1NP);                            // Interupt when low side is on.
     TIM3->CR1 |= TIM_CR1_CEN;                                   // enable TIM4
+
+    TIM3->CCMR1 |= 0x7060;
 }
 
-void Init_TMR2(){
+void Init_TMR2()
+{
     RCC->APB1ENR |= RCC_APB1ENR_TIM2EN;                         // enable TIM2 clock
-    
-     //ISR Setup     
+
+    //ISR Setup
     NVIC_EnableIRQ(TIM2_IRQn);                                  //Enable TIM2 IRQ
 
     TIM2->DIER |= TIM_DIER_UIE;                                 // enable update interrupt
     TIM2->CR1 = 0x40;                                           // CMS = 10, interrupt only when counting up // Center-aligned mode
     TIM2->CR1 |= TIM_CR1_UDIS;
-    TIM2->CR1 |= TIM_CR1_ARPE;                                  // autoreload on, 
-    TIM2->RCR |= 0x001;                                         // update event once per up/down count of TIM5
+//    TIM2->CR1 |= TIM_CR1_ARPE;                                  // autoreload on,
+    TIM2->RCR |= 0x001;                                         // update event once per up/down count of TIM2
     TIM2->EGR |= TIM_EGR_UG;
 
     TIM2->PSC = 0x00;                                            // no prescaler, timer counts up in sync with the peripheral clock
@@ -96,22 +162,49 @@
     TIM2->CR1 |= TIM_CR1_CEN;                                   // enable TIM2
 }
 
-//void Init_TMR2(){
-//    RCC->APB1ENR |= RCC_APB1ENR_TIM2EN;                         // enable TIM5 clock
-//    
-//     //ISR Setup     
-//    
-//    NVIC_EnableIRQ(TIM2_IRQn);                         //Enable TIM5 IRQ
+void Init_TMR1()
+{
+
+    RCC->APB2ENR |= RCC_APB2ENR_TIM1EN;                         // enable TIM1 clock
+//    RCC->APB2ENR |= 0x02;                                        // enable TIM8 clock
+    FastPWM pwm_v(PIN_V);
+    FastPWM pwm_w(PIN_W);
+
+    TIM1->DIER |= TIM_DIER_UIE;                                 // enable update interrupt
+    TIM1->CR1 = 0x40;                                           // CMS = 10, interrupt only when counting up // Center-aligned mode
+    TIM1->CR1 |= TIM_CR1_UDIS;
+    TIM1->CR1 |= TIM_CR1_ARPE;                                  // autoreload on,
+    TIM1->RCR |= 0x001;                                         // update event once per up/down count of TIM8
+    TIM1->EGR |= TIM_EGR_UG;
+
+
+    TIM1->CCMR1 |= 0x60;
+//    TIM1->CCMR1 |= TIM_CCMR1_OC1PE;
+//    TIM1->CCMR1 &= ~TIM_CCMR1_OC1FE;
+//    TIM1->CCMR1 |= TIM_CCMR1_OC2PE;
+//    TIM1->CCMR1 &= ~TIM_CCMR1_OC2FE;
+
+    TIM1->PSC = 0x00;                                            // no prescaler, timer counts up in sync with the peripheral clock
+    TIM1->ARR = TMR1_COUNT-1;                                          // set auto reload, 5 khz
+//    TIM1->CCER |= ~(TIM_CCER_CC1NP);                            // Interupt when low side is on.
+    TIM1->CCER |= 0x05;                                         // CC1E = 1, CC1P = 0, CC1NE = 1, CC1NP = 0
+    
+    TIM1->BDTR |= 0x8000;                                       // MOE = 1;
+    TIM1->BDTR |= 0x7F;                                         // Dead-time 7Ftick
+    
+    TIM1->CR1 |= TIM_CR1_CEN;                                   // enable TIM8
+
+
+    
+//    __HAL_RCC_GPIOA_CLK_ENABLE();
 //
-//    TIM2->DIER |= TIM_DIER_UIE;                                 // enable update interrupt
-//    TIM2->CR1 = 0x40;                                           // CMS = 10, interrupt only when counting up // Center-aligned mode
-//    TIM2->CR1 |= TIM_CR1_UDIS;
-//    TIM2->CR1 |= TIM_CR1_ARPE;                                  // autoreload on, 
-//    TIM2->RCR |= 0x001;                                         // update event once per up/down count of TIM5
-//    TIM2->EGR |= TIM_EGR_UG;
+//    GPIO_InitTypeDef GPIO_InitStruct = {0};
 //
-//    TIM2->PSC = 0x12;                                            // no prescaler, timer counts up in sync with the peripheral clock
-//    TIM2->ARR = TMR2_COUNT;                                          // set auto reload, 5 khz
-//    TIM2->CCER |= ~(TIM_CCER_CC1NP);                            // Interupt when low side is on.
-//    TIM2->CR1 |= TIM_CR1_CEN;                                   // enable TIM5
-//}
\ No newline at end of file
+//    GPIO_InitStruct.Pin = GPIO_PIN_7|GPIO_PIN_6;
+//    GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
+//    GPIO_InitStruct.Pull = GPIO_NOPULL;
+//    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
+//    GPIO_InitStruct.Alternate = GPIO_AF3_TIM1;
+//    HAL_GPIO_Init(GPIOC, &GPIO_InitStruct);
+
+}
\ No newline at end of file
--- a/INIT_HW/INIT_HW.h	Thu Apr 07 06:18:38 2022 +0000
+++ b/INIT_HW/INIT_HW.h	Mon Jun 13 08:48:55 2022 +0000
@@ -8,6 +8,7 @@
 void Init_PWM();
 void Init_TMR3();
 void Init_TMR2();
+void Init_TMR1();
 //void Init_TMR5();
 
 #endif
--- a/function_utilities/function_utilities.cpp	Thu Apr 07 06:18:38 2022 +0000
+++ b/function_utilities/function_utilities.cpp	Mon Jun 13 08:48:55 2022 +0000
@@ -234,7 +234,6 @@
 int DZ_NUM = 0;
 int one_period_end = 0;
 float Ref_Vel_Test = 0.0f;
-long TMR2_FOR_SLOW_LOGGING = 0;
 char max_check = 0;
 char min_check = 0;
 
--- a/main.cpp	Thu Apr 07 06:18:38 2022 +0000
+++ b/main.cpp	Mon Jun 13 08:48:55 2022 +0000
@@ -1,20 +1,12 @@
-//Hydraulic Control Board
+//Hydraulic Control Board Rainbow
 //distributed by Sungwoo Kim
-//       2020/12/28
-//revised by Buyoun Cho
-//       2021/04/20
-
-// 유의사항
-// 소수 적을때 뒤에 f 꼭 붙이기
-// CAN 선은 ground까지 있는 3상 선으로 써야함.
-// 전원은 12~24V 인가.
+//       2022/05/31
 
 #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"
 #include "function_utilities.h"
 #include "stm32f4xx_flash.h"
@@ -23,39 +15,34 @@
 #include <iostream>
 #include <cmath>
 
-using namespace std;
-Timer t;
 
-// dac & check ///////////////////////////////////////////
-DigitalOut check(PC_2);
-DigitalOut check_2(PC_3);
+// DAC ///////////////////////////////////////////
 AnalogOut dac_1(PA_4); // 0.0f ~ 1.0f
 AnalogOut dac_2(PA_5); // 0.0f ~ 1.0f
-AnalogIn adc1(PC_4); //pressure_1
-AnalogIn adc2(PB_0); //pressure_2
-AnalogIn adc3(PC_1); //current
+
+// ADC ///////////////////////////////////////////
+//AnalogIn adc1(PC_4); //pressure_1
+//AnalogIn adc2(PC_5); //pressure_2
+//AnalogIn adc3(PC_1); //current
+//AnalogIn adc4(PB_0); //LVDT
 
 // PWM ///////////////////////////////////////////
 float dtc_v=0.0f;
 float dtc_w=0.0f;
-DigitalOut LVDT_H(PB_4); //PWM_H
-DigitalOut LVDT_L(PB_5); //PWM_L
 
-// I2C ///////////////////////////////////////////
-I2C i2c(PC_9,PA_8); // SDA, SCL (for K22F)
-const int i2c_slave_addr1 =  0x56;  // AS5510 address
-unsigned int value; // 10bit output of reading sensor AS5510
+// LVDT ///////////////////////////////////////////
+//DigitalOut LVDT_H(PB_6);
+//DigitalOut LVDT_L(PB_7);
 
 // 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);
+
+// LED ///////////////////////////////////////////
 DigitalOut LED(PA_15);
 
-// UART ///////////////////////////////////////////
-Serial pc(PA_9,PA_10); //  _ UART
-
 // CAN ///////////////////////////////////////////
 CAN can(PB_8, PB_9, 1000000);
 CANMessage msg;
@@ -77,11 +64,6 @@
 State valve_pos;
 State valve_pos_raw;
 
-State INIT_Vout;
-State INIT_Valve_Pos;
-State INIT_Pos;
-State INIT_torq;
-
 extern int CID_RX_CMD;
 extern int CID_RX_REF_POSITION;
 extern int CID_RX_REF_OPENLOOP;
@@ -95,12 +77,6 @@
 extern int CID_TX_VALVE_POSITION;
 extern int CID_TX_SOMETHING;
 
-float temp_P_GAIN = 0.0f;
-float temp_I_GAIN = 0.0f;
-int temp_VELOCITY_COMP_GAIN = 0;
-int logging = 0;
-float valve_pos_pulse_can = 0.0f;
-
 inline float tanh_inv(float y)
 {
     if(y >= 1.0f - 0.000001f) y = 1.0f - 0.000001f;
@@ -167,12 +143,9 @@
     RCC_OscInitTypeDef RCC_OscInitStruct = {0};
     RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};
 
-    /* Configure the main internal regulator output voltage
-    */
     __HAL_RCC_PWR_CLK_ENABLE();
     __HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE1);
-    /* Initializes the CPU, AHB and APB busses clocks
-    */
+
     RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSE;
     RCC_OscInitStruct.HSEState = RCC_HSE_ON;
 //    RCC_OscInitStruct.HSICalibrationValue = RCC_HSICALIBRATION_DEFAULT;
@@ -202,36 +175,26 @@
 }
 
 
+
 int main()
 {
-    /*********************************
-    ***     Initialization
-    *********************************/
-
+    
     HAL_Init();
     SystemClock_Config();
 
     LED = 0;
-    pc.baud(9600);
 
-    // 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);
-    make_delay();
-
-    // spi init
+    // SPI INIT
     eeprom_cs = 1;
     eeprom.format(8,3);
     eeprom.frequency(5000000); //5M
     eeprom_cs = 0;
     make_delay();
 
-    enc_cs = 1;     //sw add
+    enc_cs = 1;     
     enc.format(8,0);
     enc.frequency(5000000); //10M
-    enc_cs = 0;     //sw add
+    enc_cs = 0;
 
     make_delay();
 
@@ -239,7 +202,7 @@
     spi_enc_set_init();
     make_delay();
 
-    ////// bno rom
+    //  bno rom
     spi_eeprom_write(RID_BNO, (int16_t) 0);
     make_delay();
     ////////
@@ -263,25 +226,41 @@
     make_delay();
 
     //can.reset();
-//    can.filter(msg.id, 0xFFFFF000, CANStandard);
-//    can.filter(0x050, 0xFFFFFFFF, CANStandard);
-    can.filter(0b100000000, 0b100000010, CANStandard);  //CAN ID 100~400번대 통과하게
+    can.filter(msg.id, 0xFFFFF000, CANStandard);
+//    can.filter(0b100000000, 0b100000010, CANStandard);  //CAN ID 100~400번대 통과하게
 
     // TMR3 init
     Init_TMR3();
     TIM3->CR1 ^= TIM_CR1_UDIS;
     make_delay();
 
-    // TMR5 init
+    // TMR2 init
     Init_TMR2();
     TIM2->CR1 ^= TIM_CR1_UDIS;
     make_delay();
 
+    // TMR1 init
+    Init_TMR1();
+    TIM1->CR1 ^= TIM_CR1_UDIS;
+    make_delay();
+
     //Timer priority
     NVIC_SetPriority(TIM3_IRQn, 2);
     NVIC_SetPriority(TIM4_IRQn, 3);
     NVIC_SetPriority(TIM2_IRQn, 4);
 
+//    HAL_NVIC_SetPriority(TIM2_IRQn, 4, 0);
+//    HAL_NVIC_EnableIRQ(TIM2_IRQn);
+//    /* TIM3_IRQn interrupt configuration */
+//    HAL_NVIC_SetPriority(TIM3_IRQn, 2, 0);
+//    HAL_NVIC_EnableIRQ(TIM3_IRQn);
+//    /* TIM4_IRQn interrupt configuration */
+//    HAL_NVIC_SetPriority(TIM4_IRQn, 3, 0);
+//    HAL_NVIC_EnableIRQ(TIM4_IRQn);
+//    /* CAN1_RX0_IRQn interrupt configuration */
+//    HAL_NVIC_SetPriority(CAN1_RX0_IRQn, 14, 0);
+//    HAL_NVIC_EnableIRQ(CAN1_RX0_IRQn);
+
 
     //DAC init
     if (SENSING_MODE == 0) {
@@ -305,29 +284,16 @@
             ID_index_array[i] =  (i+1) * 0.5f;
     }
 
-    //pwm
-    TIM4->CCR2 = (PWM_ARR)*(1.0f-0.0f);
-    TIM4->CCR1 = (PWM_ARR)*(1.0f-0.0f);
-    LVDT_H = 0;
-    LVDT_L = 0;
-
     /************************************
     ***     Program is operating!
     *************************************/
     while(1) {
+        
+//            if (LED > 0) LED = 0;
+//            else LED = 1;
+        TIM1->CCR1 = (TMR1_COUNT)*(0.7f);
 
-        // UART example
-//        if(timer_while==100000) {
-//            timer_while = 0;
-//            pc.printf("%f\n", value);
-//        }
-//        timer_while ++;
 
-        //i2c for SW valve
-//        if(OPERATING_MODE == 5) {
-//            read_field(i2c_slave_addr1);
-//            if(DIR_VALVE_ENC < 0) value = 1023 - value;
-//        }
     }
 }
 
@@ -375,7 +341,7 @@
     } else if(REF_VALVE_POS < VALVE_MIN_POS) {
         REF_VALVE_POS = VALVE_MIN_POS;
     }
-    valve_pos_err = (float) (REF_VALVE_POS - value);
+    valve_pos_err = (float) (REF_VALVE_POS - valve_pos.sen);
     valve_pos_err_diff = valve_pos_err - valve_pos_err_old;
     valve_pos_err_old = valve_pos_err;
     valve_pos_err_sum += valve_pos_err;
@@ -473,7 +439,6 @@
             }
         }
     }
-
     return PWM_duty;
 }
 
@@ -483,43 +448,65 @@
                             TIMER INTERRUPT
 *******************************************************************************/
 
+
+//------------------------------------------------
+//     TMR3 : LVDT 1kHz
+//-----------------------------------------------
 float LVDT_new = 0.0f;
 float LVDT_old = 0.0f;
 float LVDT_f_cut = 1000.0f;
 float LVDT_LPF = 0.0f;
 float LVDT_sum = 0.0f;
-float LVDT_can[100] = {0.0f};
 
 extern "C" void TIM3_IRQHandler(void)
 {
     if (TIM3->SR & TIM_SR_UIF ) {
 
-//        if (LED > 0) LED = 0;
-//        else LED = 1;
+////        if (LED > 0) LED = 0;
+////        else LED = 1;
+//
+//        LVDT_sum = 0.0f;
+//
+//
+//        LVDT_L = 0;
+//        LVDT_H = 1;
+//
+////        LED = 1;
+//
+//        for (int ij = 0; ij<150; ij++) {
+//            ADC1->CR2  |= 0x40000000;
+//            LVDT_new = ((float)ADC1->DR) - 2047.5f;
+//            LVDT_sum = LVDT_sum + LVDT_new;
+//        }
+//
+////        LED = 0;
+//
+//        LVDT_H = 0;
+//        LVDT_L = 1;
 
-        LVDT_sum = 0.0f;
-
-        LVDT_H = 1;
-        LVDT_L = 0;
 
-        for (int ij = 0; ij<200; ij++) {
-//            LED = 1;
-            ADC1->CR2  |= 0x40000000;
-            LVDT_new = ((float)ADC1->DR) - 2047.5f;
-            LVDT_sum = LVDT_sum + LVDT_new;
-//            LED = 0;
-        }
+
+
+//        LVDT_new = LVDT_sum * 0.01f*2.0f;
+//
+//        float alpha_LVDT = 1.0f/(1.0f+TMR_FREQ_1k/(2.0f*PI*300.0f));
+//        LVDT_LPF = (1.0f-alpha_LVDT) * LVDT_LPF + alpha_LVDT * LVDT_new;
+//        valve_pos.sen = LVDT_LPF;
+//        if(DIR_VALVE_ENC < 0) valve_pos.sen = 0.0f - valve_pos.sen;
 
-        LVDT_H = 0;
-        LVDT_L = 0;
+//        TIM4->CCR2 = (PWM_ARR)*(0.95f);
+//        TIM4->CCR1 = (PWM_ARR)*(0.975f);
+
+//        TIM8->CCR1 = (TMR8_COUNT)*(0.95f);
+//        TIM8->CCR2 = (TMR8_COUNT)*(0.975f);
+
 
-//        LED = 0;
-        LVDT_new = LVDT_sum * 0.005f;
+//        PWM_H2 = 0;
+//        PWM_L2 = 1;
 
-        float alpha_LVDT = 1.0f/(1.0f+TMR_FREQ_1k/(2.0f*PI*300.0f));
-        LVDT_LPF = (1.0f-alpha_LVDT) * LVDT_LPF + alpha_LVDT * LVDT_new;
-        valve_pos.sen = LVDT_LPF;
-        if(DIR_VALVE_ENC < 0) valve_pos.sen = 0.0f - valve_pos.sen;
+//        TIM3->CCR2 = (TMR3_COUNT)*(0.95f);
+//        TIM3->CCR1 = (TMR3_COUNT)*(0.975f);
+
 
     }
     TIM3->SR = 0x0;  // reset the status register
@@ -538,9 +525,6 @@
 {
     if (TIM4->SR & TIM_SR_UIF ) {
 
-//        if (LED > 0) LED = 0;
-//        else LED = 1;
-
         // Current ===================================================
         //ADC3->CR2  |= 0x40000000;                        // adc _ 12bit
 
@@ -578,6 +562,51 @@
         }
         */
 
+//        //FORWARD
+//        if(CNT_TMR4 < 2)
+//        {
+//            PWM_L2 = 0;
+//            PWM_H1 = 0;
+//
+//            PWM_H2 = 1;
+//            PWM_L1 = 1;
+//        } else if(CNT_TMR4 < 50)
+//        {
+//            PWM_H2 = 0;
+//            PWM_H1 = 0;
+//
+//            PWM_L2 = 1;
+//            PWM_L1 = 1;
+//        } else
+//        {
+//            CNT_TMR4 = 0;
+//        }
+
+
+//        //BACKWARD
+//        if(CNT_TMR4 < 2)
+//        {
+//            PWM_H2 = 0;
+//            PWM_L1 = 0;
+//
+//            PWM_L2 = 1;
+//            PWM_H1 = 1;
+//        } else if(CNT_TMR4 < 50)
+//        {
+//            PWM_H2 = 0;
+//            PWM_H1 = 0;
+//
+//            PWM_L2 = 1;
+//            PWM_L1 = 1;
+//        } else
+//        {
+//            CNT_TMR4 = 0;
+//        }
+
+
+
+
+
         CNT_TMR4++;
     }
     TIM4->SR = 0x0;  // reset the status register
@@ -588,7 +617,6 @@
 float FREQ_TMR5 = (float)FREQ_5k;
 float DT_TMR5 = (float)DT_5k;
 int cnt_trans = 0;
-double VALVE_POS_RAW_FORCE_FB_LOGGING = 0.0f;
 int can_rest =0;
 float force_ref_act_can = 0.0f;
 
@@ -828,7 +856,6 @@
                     data_num = 0;
                 } else if(VALVE_ID_timer < TMR_FREQ_5k*3) {
                     data_num = data_num + 1;
-//                    VALVE_POS_TMP = VALVE_POS_TMP + value;
                     VALVE_POS_TMP = VALVE_POS_TMP + valve_pos.sen;
                 } else if(VALVE_ID_timer == TMR_FREQ_5k*3) {
                     Vout.ref = 0.0f;
@@ -884,7 +911,7 @@
                     } else if(VALVE_DZ_timer < (int) (4.0f * (float) TMR_FREQ_5k)) {
                         Vout.ref = (float) P_GAIN_JOINT_POSITION * (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen)*(float) 50.0f;
                         data_num = data_num + 1;
-                        VALVE_POS_TMP = VALVE_POS_TMP + value;
+                        VALVE_POS_TMP = VALVE_POS_TMP + valve_pos.sen;
                     } else if(VALVE_DZ_timer == (int) (4.0f * (float) TMR_FREQ_5k)) {
                         Vout.ref = (float) P_GAIN_JOINT_POSITION * (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen)*(float) 50.0f;
                         DDV_POS_AVG = VALVE_POS_TMP / data_num;
@@ -923,7 +950,7 @@
                     }
                 } else {
                     if((DZ_case == -1 && DZ_NUM == 1) | (DZ_case == 1 && DZ_NUM == 1)) {
-                        if(VALVE_DZ_timer < (int) (1.0 * (float) TMR_FREQ_5k)) {
+                        if(VALVE_DZ_timer < (int) (1.0f * (float) TMR_FREQ_5k)) {
                             Vout.ref = (float) P_GAIN_JOINT_POSITION * (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen)*(float) 50.0f;
                         } else if(VALVE_DZ_timer == (int) (1.0f * (float) TMR_FREQ_5k)) {
                             START_POS = pos.sen;
@@ -1096,18 +1123,18 @@
                     valve_pos_ref = 10000.0f;
                 }
                 if(valve_pos_ref >= 0) {
-                    valve_pos_raw.ref = (double)VALVE_ELECTRIC_CENTER + (double)valve_pos_ref * ((double)VALVE_MAX_POS-(double)VALVE_ELECTRIC_CENTER)/10000.0f;
+                    valve_pos_raw.ref = (float)VALVE_ELECTRIC_CENTER + (float)valve_pos_ref * ((float)VALVE_MAX_POS-(float)VALVE_ELECTRIC_CENTER)/10000.0f;
                 } else {
-                    valve_pos_raw.ref = (double)VALVE_ELECTRIC_CENTER - (double)valve_pos_ref * ((double)VALVE_MIN_POS-(double)VALVE_ELECTRIC_CENTER)/10000.0f;
+                    valve_pos_raw.ref = (float)VALVE_ELECTRIC_CENTER - (float)valve_pos_ref * ((float)VALVE_MIN_POS-(float)VALVE_ELECTRIC_CENTER)/10000.0f;
                 }
 
                 VALVE_POS_CONTROL(valve_pos_raw.ref);
 
                 ref_array[cnt_step_test] = valve_pos_ref;
-                if(value>=(float) VALVE_ELECTRIC_CENTER) {
-                    pos_array[cnt_step_test] = 10000.0f*((double)value - (double)VALVE_ELECTRIC_CENTER)/((double)VALVE_MAX_POS - (double)VALVE_ELECTRIC_CENTER);
+                if(valve_pos.sen >= (float) VALVE_ELECTRIC_CENTER) {    
+                    pos_array[cnt_step_test] = 10000.0f*((float)valve_pos.sen - (float)VALVE_ELECTRIC_CENTER)/((float)VALVE_MAX_POS - (float)VALVE_ELECTRIC_CENTER);
                 } else {
-                    pos_array[cnt_step_test] = -10000.0f*((double)value - (double)VALVE_ELECTRIC_CENTER)/((double)VALVE_MIN_POS - (double)VALVE_ELECTRIC_CENTER);
+                    pos_array[cnt_step_test] = -10000.0f*((float)valve_pos.sen - (float)VALVE_ELECTRIC_CENTER)/((float)VALVE_MIN_POS - (float)VALVE_ELECTRIC_CENTER);
                 }
 
                 CONTROL_MODE = MODE_VALVE_OPEN_LOOP;
@@ -1119,17 +1146,10 @@
                     CONTROL_UTILITY_MODE = MODE_SEND_OVER;
                     CONTROL_MODE = MODE_NO_ACT;
                 }
-//                if (cnt_step_test > (int) (2.0f * (float) TMR_FREQ_5k))
-//                {
-//                    CONTROL_UTILITY_MODE = MODE_NO_ACT;
-//                    CONTROL_MODE = MODE_NO_ACT;
-//                    CAN_TX_PWM((int16_t) (1)); //1300
-//                }
 
                 break;
             }
             case MODE_SEND_OVER: {
-                CAN_TX_TORQUE((int16_t) (buffer_data_size)); //1300
                 CONTROL_UTILITY_MODE = MODE_NO_ACT;
                 CONTROL_MODE = MODE_NO_ACT;
                 break;
@@ -1138,18 +1158,21 @@
             case MODE_FREQ_TEST: {
                 float valve_pos_ref = 2500.0f * sin(2.0f * 3.141592f * freq_test_valve_ref * (float) cnt_freq_test * DT_TMR5);
                 if(valve_pos_ref >= 0) {
-                    valve_pos_raw.ref = (double)VALVE_ELECTRIC_CENTER + (double)valve_pos_ref * ((double)VALVE_MAX_POS-(double)VALVE_ELECTRIC_CENTER)/10000.0f;
+                    valve_pos_raw.ref = (float)VALVE_ELECTRIC_CENTER + (float)valve_pos_ref * ((float)VALVE_MAX_POS-(float)VALVE_ELECTRIC_CENTER)/10000.0f;
                 } else {
-                    valve_pos_raw.ref = (double)VALVE_ELECTRIC_CENTER - (double)valve_pos_ref * ((double)VALVE_MIN_POS-(double)VALVE_ELECTRIC_CENTER)/10000.0f;
+                    valve_pos_raw.ref = (double)VALVE_ELECTRIC_CENTER - (float)valve_pos_ref * ((float)VALVE_MIN_POS-(float)VALVE_ELECTRIC_CENTER)/10000.0f;
                 }
 
                 VALVE_POS_CONTROL(valve_pos_raw.ref);
 
                 ref_array[cnt_freq_test] = valve_pos_ref;
-                if(value>=(float) VALVE_ELECTRIC_CENTER) {
-                    pos_array[cnt_freq_test] = 10000.0f*((double)value - (double)VALVE_ELECTRIC_CENTER)/((double)VALVE_MAX_POS - (double)VALVE_ELECTRIC_CENTER);
+//                if(value>=(float) VALVE_ELECTRIC_CENTER) {
+                if(valve_pos.sen>=(float) VALVE_ELECTRIC_CENTER) {    
+//                    pos_array[cnt_freq_test] = 10000.0f*((float)value - (float)VALVE_ELECTRIC_CENTER)/((float)VALVE_MAX_POS - (float)VALVE_ELECTRIC_CENTER);
+                    pos_array[cnt_freq_test] = 10000.0f*((float)valve_pos.sen - (float)VALVE_ELECTRIC_CENTER)/((float)VALVE_MAX_POS - (float)VALVE_ELECTRIC_CENTER);
                 } else {
-                    pos_array[cnt_freq_test] = -10000.0f*((double)value - (double)VALVE_ELECTRIC_CENTER)/((double)VALVE_MIN_POS - (double)VALVE_ELECTRIC_CENTER);
+//                    pos_array[cnt_freq_test] = -10000.0f*((float)value - (float)VALVE_ELECTRIC_CENTER)/((float)VALVE_MIN_POS - (float)VALVE_ELECTRIC_CENTER);
+                    pos_array[cnt_freq_test] = -10000.0f*((float)valve_pos.sen - (float)VALVE_ELECTRIC_CENTER)/((float)VALVE_MIN_POS - (float)VALVE_ELECTRIC_CENTER);
                 }
 
                 CONTROL_MODE = MODE_VALVE_OPEN_LOOP;
@@ -1162,7 +1185,6 @@
                     if (freq_test_valve_ref >= 400) {
                         CONTROL_UTILITY_MODE = MODE_NO_ACT;
                         CONTROL_MODE = MODE_NO_ACT;
-                        CAN_TX_VOUT((int16_t) (1)); //1300
                     }
                     CONTROL_MODE = MODE_NO_ACT;
                     CONTROL_UTILITY_MODE = MODE_SEND_OVER;
@@ -1281,7 +1303,6 @@
 
                     float I_MAX = 10.0f; // Maximum Current : 10mA
                     // Anti-windup for FT
-                    //                if (I_GAIN_JOINT_TORQUE != 0.0f) {
                     if (I_GAIN_JOINT_TORQUE > 0.001f) {
                         float Ka = 2.0f;
                         if (I_REF > I_MAX) {
@@ -1305,7 +1326,6 @@
                 } else {    //SW valve
                     float Valve_pos_MAX = 10000.0f; // Maximum Valve Pos : 10000
                     // Anti-windup for FT
-                    //                if (I_GAIN_JOINT_TORQUE != 0.0f) {
                     if (I_GAIN_JOINT_TORQUE > 0.001f) {
                         float Ka = 2.0f;
                         if (valve_pos_pulse > Valve_pos_MAX) {
@@ -1332,7 +1352,6 @@
 //                        valve_pos.ref = -valve_pos_pulse/10000.0f * (VALVE_MIN_POS-VALVE_DEADZONE_MINUS) + VALVE_DEADZONE_MINUS;
 //                    }
                     VALVE_POS_CONTROL_DZ(valve_pos_pulse);
-                    valve_pos_pulse_can = valve_pos_pulse;
                     V_out = Vout.ref;
                 }
                 break;
@@ -1343,79 +1362,6 @@
                 break;
             }
 
-//            case MODE_JOINT_ADAPTIVE_BACKSTEPPING: {
-//
-//                float Va = (1256.6f + Amm * pos.sen/(float)(ENC_PULSE_PER_POSITION)) * 0.000000001f; // 4mm pipe * 100mm + (25mm Cylinder 18mm Rod) * x,      unit : m^3
-//                float Vb = (1256.6f + Amm  * (79.0f - pos.sen/(float)(ENC_PULSE_PER_POSITION))) * 0.000000001f; // 4mm pipe * 100mm + (25mm Cylinder 18mm Rod) * (79.0mm-x),      unit : m^3
-//
-//                V_adapt = 1.0f / (1.0f/Va + 1.0f/Vb); //initial 0.0000053f
-//
-//                //float f3 = -Amm*Amm*beta*0.000001f*0.000001f/V_adapt * vel.sen/(float)(ENC_PULSE_PER_POSITION)*0.001f; // unit : N/s    //xdot=10mm/s일때 -137076
-//                float f3_hat = -a_hat * vel.sen/(float)(ENC_PULSE_PER_POSITION)*0.001f; // unit : N/s    //xdot=10mm/s일때 -137076
-//
-//                float g3_prime = 0.0f;
-//                if (torq.sen > Amm*(Ps-Pt)*0.000001f) {
-//                    g3_prime = 1.0f;
-//                } else if (torq.sen < -Amm*(Ps-Pt)*0.000001f) {
-//                    g3_prime = -1.0f;
-//                } else {
-//                    if ((value-VALVE_CENTER) > 0) {
-//                        g3_prime = sqrt(Ps-Pt-torq.sen/Amm*1000000.0f);
-////                        g3_prime = sqrt(Ps-Pt);
-//                    } else {
-//                        g3_prime = sqrt(Ps-Pt+torq.sen/Amm*1000000.0f);
-////                        g3_prime = sqrt(Ps-Pt);
-//                    }
-//                }
-//                float tau = 0.01f;
-//                float K_valve = 0.0004f;
-//
-//                float x_v = 0.0f;   //x_v : -1~1
-//                if(value>=VALVE_CENTER) {
-//                    x_v = 1.0f*((double)value - (double)VALVE_CENTER)/((double)VALVE_MAX_POS - (double)VALVE_CENTER);
-//                } else {
-//                    x_v = -1.0f*((double)value - (double)VALVE_CENTER)/((double)VALVE_MIN_POS - (double)VALVE_CENTER);
-//                }
-//                float f4 = -x_v/tau;
-//                float g4 = K_valve/tau;
-//
-//                float torq_ref_dot = torq.ref_diff * 500.0f;
-//
-//                pos.err = (pos.ref - pos.sen)/(float)(ENC_PULSE_PER_POSITION); //[mm]
-//                vel.err = (0.0f - vel.sen)/(float)(ENC_PULSE_PER_POSITION); //[mm/s]
-//                pos.err_sum += pos.err/(float) TMR_FREQ_5k; //[mm]
-//
-//                torq.err = torq.ref - torq.sen; //[N]
-//                torq.err_sum += torq.err/(float) TMR_FREQ_5k; //[N]
-//
-//                float k3 = 2000.0f; //2000  //20000
-//                float k4 = 10.0f;
-//                float rho3 = 3.2f;
-//                float rho4 = 10000000.0f;  //25000000.0f;
-//                float x_4_des = (-f3_hat + torq_ref_dot - k3*(-torq.err))/(gamma_hat*g3_prime);
-//                if (x_4_des > 1) x_4_des = 1;
-//                else if (x_4_des < -1) x_4_des = -1;
-//
-//                if (x_4_des > 0) {
-//                    valve_pos.ref = x_4_des * (float)(VALVE_MAX_POS - VALVE_CENTER) + (float) VALVE_CENTER;
-//                } else {
-//                    valve_pos.ref = x_4_des * (float)(VALVE_CENTER - VALVE_MIN_POS) + (float) VALVE_CENTER;
-//                }
-//
-//                float x_4_des_dot = (x_4_des - x_4_des_old)*(float) TMR_FREQ_5k;
-//                x_4_des_old = x_4_des;
-//                V_out = (-f4 + x_4_des_dot - k4*(x_v-x_4_des)- rho3/rho4*gamma_hat*g3_prime*(-torq.err))/g4;
-//
-//                float rho_a = 0.00001f;
-//                float a_hat_dot = -rho3/rho_a*vel.sen/(float)(ENC_PULSE_PER_POSITION)*0.001f*(-torq.err);
-//                a_hat = a_hat + a_hat_dot / (float) TMR_FREQ_5k;
-//
-//                if(a_hat > -3000000.0f) a_hat = -3000000.0f;
-//                else if(a_hat < -30000000.0f) a_hat = -30000000.0f;
-//
-//                break;
-//            }
-
             default:
                 break;
         }
@@ -1437,7 +1383,6 @@
                 I_ERR = I_REF_fil_DZ - (double)cur.sen;
                 I_ERR_INT = I_ERR_INT + (I_ERR) * 0.0002f;
 
-
                 // Moog Valve Current Control Gain
                 double R_model = 500.0f; // ohm
                 double L_model = 1.2f;
@@ -1455,7 +1400,6 @@
                 }
 
                 double FF_gain = 1.0f;
-
                 VALVE_PWM_RAW = KP_I * 2.0f * I_ERR + KI_I * 2.0f* I_ERR_INT;
                 I_REF_fil_diff = I_REF_fil_DZ - I_REF_fil_old;
                 I_REF_fil_old = I_REF_fil_DZ;
@@ -1527,54 +1471,38 @@
             dtc_w=0.0f;
         }
 
-        //pwm
-        TIM4->CCR2 = (PWM_ARR)*(1.0f-dtc_v);
-        TIM4->CCR1 = (PWM_ARR)*(1.0f-dtc_w);
 
         ////////////////////////////////////////////////////////////////////////////
         //////////////////////  Data transmission through CAN //////////////////////
         ////////////////////////////////////////////////////////////////////////////
 
-        if (TMR2_COUNT_CAN_TX % (int) ((int) TMR_FREQ_5k/CAN_FREQ) == 0) {
 
+//        if (TMR2_COUNT_CAN_TX % (int) ((int) TMR_FREQ_5k/CAN_FREQ) == 0) {
+        if (TMR2_COUNT_CAN_TX % (int) ((int) TMR_FREQ_5k/500) == 0) {
 
             // Position, Velocity, and Torque (ID:1200)
-//            if (flag_data_request[0] == LOW) {
+            if (flag_data_request[0] == HIGH) {
 
-//                if ((OPERATING_MODE & 0b01) == 0) { // Rotary Actuator
-//                    CAN_TX_POSITION_FT((int16_t) (pos.sen*200.0f), (int16_t) (vel.sen*20.0f), (int16_t) (torq.sen*TORQUE_SENSOR_PULSE_PER_TORQUE*10.0f));
-//                    CAN_TX_POSITION_FT((int16_t) (PRES_B_VREF*10.0f*200.0f), (int16_t) (vel.sen*20.0f), (int16_t) (pres_B.sen*TORQUE_SENSOR_PULSE_PER_TORQUE*10.0f));
+                if ((OPERATING_MODE & 0b01) == 0) { // Rotary Actuator
+                    CAN_TX_POSITION_FT((int16_t) (pos.sen*200.0f), (int16_t) (vel.sen*20.0f), (int16_t) (torq.sen*TORQUE_SENSOR_PULSE_PER_TORQUE*10.0f));
 
-//                } else if ((OPERATING_MODE & 0b01) == 1) { // Linear Actuator
-//                    CAN_TX_POSITION_FT((int16_t) (pos.sen*200.0f), (int16_t) (vel.sen*20.0f), (int16_t) (force.sen*TORQUE_SENSOR_PULSE_PER_TORQUE*10.0f));
-            CAN_TX_POSITION_FT((int16_t) (7.0f*200.0f), (int16_t) (vel.sen*20.0f), (int16_t) (valve_pos.sen));
-//                }
-//            }
+                } else if ((OPERATING_MODE & 0b01) == 1) { // Linear Actuator
+                    CAN_TX_POSITION_FT((int16_t) (pos.sen*200.0f), (int16_t) (vel.sen*20.0f), (int16_t) (force.sen*TORQUE_SENSOR_PULSE_PER_TORQUE*10.0f));
+                }
+            }
 
             // Valve Position (ID:1300)
             if (flag_data_request[1] == HIGH) {
-                CAN_TX_PWM((int16_t)(cur.sen/mA_PER_pulse));
-//                CAN_TX_PWM((int16_t)(TORQUE_SENSOR_PULSE_PER_TORQUE*10000.0f));
+                if (((OPERATING_MODE&0b110)>>1) == 0 || ((OPERATING_MODE&0b110)>>1) == 1) { //Moog Valve or KNR Valve
+                    CAN_TX_PWM((int16_t)(cur.sen/mA_PER_pulse));
+                } else {
+                    CAN_TX_PWM((int16_t)(valve_pos.sen));
+                }
             }
 
-            // Others : Pressure A, B, Supply Pressure, etc. (for Debugging)  (ID:1400)
+            // Others : SW (ID:1400)
             if (flag_data_request[2] == HIGH) {
-                float valve_pos_can = 0.0f;
-                if(value >= VALVE_ELECTRIC_CENTER) {
-                    valve_pos_can = 10000.0f*((float)value-(float)VALVE_ELECTRIC_CENTER)/((float)VALVE_MAX_POS-(float)VALVE_ELECTRIC_CENTER);
-                } else {
-                    valve_pos_can = -10000.0f*((float)value -(float)VALVE_ELECTRIC_CENTER)/((float)VALVE_MIN_POS-(float)VALVE_ELECTRIC_CENTER);
-                }
-                float valve_pos_ref_can = 0.0f;
-                if(valve_pos.ref >= VALVE_ELECTRIC_CENTER) {
-                    valve_pos_ref_can = 10000.0f*((float)valve_pos.ref-(float)VALVE_ELECTRIC_CENTER)/((float)VALVE_MAX_POS-(float)VALVE_ELECTRIC_CENTER);
-                } else {
-                    valve_pos_ref_can = -10000.0f*((float)valve_pos.ref -(float)VALVE_ELECTRIC_CENTER)/((float)VALVE_MIN_POS-(float)VALVE_ELECTRIC_CENTER);
-                }
-
-                valve_pos_ref_can = (float)valve_pos.ref;
-
-                CAN_TX_CURRENT((int16_t) valve_pos_can, (int16_t) valve_pos_pulse_can);
+                CAN_TX_CURRENT((int16_t) valve_pos.sen, (int16_t) valve_pos.ref);
             }
 
             TMR2_COUNT_CAN_TX = 0;
@@ -1583,5 +1511,4 @@
 
     }
     TIM2->SR = 0x0;  // reset the status register
-
-}
\ No newline at end of file
+}
--- a/setting.h	Thu Apr 07 06:18:38 2022 +0000
+++ b/setting.h	Mon Jun 13 08:48:55 2022 +0000
@@ -2,17 +2,25 @@
 #include "FastPWM.h"
 
 // pwm 
-#define PIN_V       PB_7
-#define PIN_W       PB_6
+//#define PIN_V       PB_4
+//#define PIN_W       PB_5
+//#define PIN_V      PC_6
+//#define PIN_W      PC_7
+#define PIN_V      PA_8
+#define PIN_W      PA_7
+//#define PIN_V       PB_6
+//#define PIN_W       PB_7
 //#define PWM_ARR     0x465       // loop 80k, pwm 40k 
 //#define PWM_ARR     0x8CA       // loop 40k, pwm 20k
 #define PWM_ARR     0x1194      // loop 20k, pwm 10k 원래이거
 //#define PWM_ARR     0x2328      // loop 10k, pwm 5k
 //#define PWM_ARR     0xAFC8        // loop 2 k, pwm 1k
 
-#define TMR3_COUNT  0xAFC8      // loop 2 k, pwm 1k
+//#define TMR3_COUNT  0xAFC8      // loop 2 k, pwm 1k
+#define TMR3_COUNT  0x1194      
 //#define TMR2_COUNT  0x2710      // loop 500hz with prescale 18
 #define TMR2_COUNT  0x4650      // loop 5 k
+#define TMR1_COUNT  0x1194  
 
 #define FREQ_500    500.0f
 #define FREQ_1k     1000.0f
@@ -31,19 +39,12 @@
 #define             TMR_FREQ_5k         5000
 #define             TMR_FREQ_1k         1000
 
-extern DigitalOut check;
-extern DigitalOut check_2;
 extern AnalogOut dac_1;
 extern AnalogOut dac_2;
 
 extern float dtc_v;
 extern float dtc_w;
 
-// I2C
-extern I2C i2c; // SDA, SCL (for K22F)
-extern const int i2c_slave_addr1; 
-extern unsigned int value; // 10bit output of reading sensor AS5510
-
 // SPI
 extern SPI eeprom; //(SPI_MOSI, SPI_MISO, SPI_SCK);
 extern DigitalOut eeprom_cs;
@@ -329,7 +330,6 @@
 extern int DZ_NUM;
 extern int one_period_end;
 extern float Ref_Vel_Test;
-extern long TMR2_FOR_SLOW_LOGGING;
 //extern int velcount;
 extern char max_check;
 extern char min_check;