Sungwoo Kim / Mbed 2 deprecated HydraulicControlBoard_Rainbow_v1_2

rainbow

Dependencies:   mbed FastPWM

main.cpp@257:c93d3eabff75, 2022-03-31 (annotated)

Committer:
Lightvalve
Date:
Thu Mar 31 02:39:48 2022 +0000
Revision:
257:c93d3eabff75
Parent:
239:8ac5c6162bc1 
LVDT

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Lightvalve 224:985dba42f261 1 //Hydraulic Control Board
Lightvalve 224:985dba42f261 2 //distributed by Sungwoo Kim
Lightvalve 224:985dba42f261 3 // 2020/12/28
jobuuu 228:763bbefcb79f 4 //revised by Buyoun Cho
Lightvalve 235:c831718303c9 5 // 2021/04/20
Lightvalve 224:985dba42f261 6
Lightvalve 224:985dba42f261 7 // 유의사항
Lightvalve 224:985dba42f261 8 // 소수 적을때 뒤에 f 꼭 붙이기
Lightvalve 224:985dba42f261 9 // CAN 선은 ground까지 있는 3상 선으로 써야함.
Lightvalve 224:985dba42f261 10 // 전원은 12~24V 인가.
Lightvalve 224:985dba42f261 11
GiJeongKim 0:51c43836c1d7 12 #include "mbed.h"
GiJeongKim 0:51c43836c1d7 13 #include "FastPWM.h"
GiJeongKim 0:51c43836c1d7 14 #include "INIT_HW.h"
jobuuu 2:a1c0a37df760 15 #include "function_CAN.h"
GiJeongKim 0:51c43836c1d7 16 #include "SPI_EEP_ENC.h"
GiJeongKim 0:51c43836c1d7 17 #include "I2C_AS5510.h"
GiJeongKim 0:51c43836c1d7 18 #include "setting.h"
Lightvalve 11:82d8768d7351 19 #include "function_utilities.h"
Lightvalve 16:903b5a4433b4 20 #include "stm32f4xx_flash.h"
Lightvalve 169:645207e160ca 21 #include "FlashWriter.h"
Lightvalve 170:42c938a40313 22 #include <string>
Lightvalve 170:42c938a40313 23 #include <iostream>
Lightvalve 179:d5377766d7ea 24 #include <cmath>
Lightvalve 61:bc8c8270f0ab 25
Lightvalve 61:bc8c8270f0ab 26 using namespace std;
Lightvalve 61:bc8c8270f0ab 27 Timer t;
GiJeongKim 0:51c43836c1d7 28
jobuuu 7:e9086c72bb22 29 // dac & check ///////////////////////////////////////////
GiJeongKim 0:51c43836c1d7 30 DigitalOut check(PC_2);
GiJeongKim 0:51c43836c1d7 31 DigitalOut check_2(PC_3);
jobuuu 230:2c3e5ecbe7e1 32 AnalogOut dac_1(PA_4); // 0.0f ~ 1.0f
jobuuu 230:2c3e5ecbe7e1 33 AnalogOut dac_2(PA_5); // 0.0f ~ 1.0f
Lightvalve 14:8e7590227d22 34 AnalogIn adc1(PC_4); //pressure_1
Lightvalve 14:8e7590227d22 35 AnalogIn adc2(PB_0); //pressure_2
Lightvalve 14:8e7590227d22 36 AnalogIn adc3(PC_1); //current
GiJeongKim 0:51c43836c1d7 37
jobuuu 7:e9086c72bb22 38 // PWM ///////////////////////////////////////////
Lightvalve 30:8d561f16383b 39 float dtc_v=0.0f;
Lightvalve 30:8d561f16383b 40 float dtc_w=0.0f;
GiJeongKim 0:51c43836c1d7 41
jobuuu 7:e9086c72bb22 42 // I2C ///////////////////////////////////////////
GiJeongKim 0:51c43836c1d7 43 I2C i2c(PC_9,PA_8); // SDA, SCL (for K22F)
Lightvalve 224:985dba42f261 44 const int i2c_slave_addr1 = 0x56; // AS5510 address
GiJeongKim 0:51c43836c1d7 45 unsigned int value; // 10bit output of reading sensor AS5510
GiJeongKim 0:51c43836c1d7 46
jobuuu 7:e9086c72bb22 47 // SPI ///////////////////////////////////////////
Lightvalve 170:42c938a40313 48 SPI eeprom(PB_15, PB_14, PB_13); // EEPROM //(SPI_MOSI, SPI_MISO, SPI_SCK);
Lightvalve 170:42c938a40313 49 DigitalOut eeprom_cs(PB_12);
GiJeongKim 0:51c43836c1d7 50 SPI enc(PC_12,PC_11,PC_10);
GiJeongKim 0:51c43836c1d7 51 DigitalOut enc_cs(PD_2);
Lightvalve 11:82d8768d7351 52 DigitalOut LED(PA_15);
GiJeongKim 0:51c43836c1d7 53
jobuuu 7:e9086c72bb22 54 // UART ///////////////////////////////////////////
GiJeongKim 0:51c43836c1d7 55 Serial pc(PA_9,PA_10); // _ UART
GiJeongKim 0:51c43836c1d7 56
jobuuu 7:e9086c72bb22 57 // CAN ///////////////////////////////////////////
GiJeongKim 0:51c43836c1d7 58 CAN can(PB_8, PB_9, 1000000);
GiJeongKim 0:51c43836c1d7 59 CANMessage msg;
Lightvalve 11:82d8768d7351 60 void onMsgReceived()
Lightvalve 11:82d8768d7351 61 {
Lightvalve 11:82d8768d7351 62 CAN_RX_HANDLER();
Lightvalve 11:82d8768d7351 63 }
jobuuu 2:a1c0a37df760 64
jobuuu 7:e9086c72bb22 65 // Variables ///////////////////////////////////////////
jobuuu 7:e9086c72bb22 66 State pos;
jobuuu 7:e9086c72bb22 67 State vel;
jobuuu 7:e9086c72bb22 68 State Vout;
jobuuu 230:2c3e5ecbe7e1 69 State force;
jobuuu 230:2c3e5ecbe7e1 70 State torq; // unit : N
Lightvalve 207:c70c5a9f17dd 71 State torq_dot;
jobuuu 230:2c3e5ecbe7e1 72 State pres_A; // unit : bar
jobuuu 7:e9086c72bb22 73 State pres_B;
jobuuu 230:2c3e5ecbe7e1 74 State cur; // unit : mA
Lightvalve 14:8e7590227d22 75 State valve_pos;
Lightvalve 238:4e660aa77eb7 76 State valve_pos_raw;
Lightvalve 14:8e7590227d22 77
Lightvalve 14:8e7590227d22 78 State INIT_Vout;
Lightvalve 14:8e7590227d22 79 State INIT_Valve_Pos;
Lightvalve 14:8e7590227d22 80 State INIT_Pos;
Lightvalve 14:8e7590227d22 81 State INIT_torq;
jobuuu 5:a4319f79457b 82
Lightvalve 19:23b7c1ad8683 83 extern int CID_RX_CMD;
Lightvalve 19:23b7c1ad8683 84 extern int CID_RX_REF_POSITION;
hyhoon 227:699c3e572283 85 extern int CID_RX_REF_OPENLOOP;
Lightvalve 45:35fa6884d0c6 86 extern int CID_RX_REF_PWM;
Lightvalve 19:23b7c1ad8683 87
Lightvalve 19:23b7c1ad8683 88 extern int CID_TX_INFO;
hyhoon 227:699c3e572283 89 extern int CID_TX_POS_VEL_TORQ;
hyhoon 227:699c3e572283 90 extern int CID_TX_PWM;
hyhoon 227:699c3e572283 91 extern int CID_TX_CURRENT;
Lightvalve 19:23b7c1ad8683 92 extern int CID_TX_VOUT;
Lightvalve 19:23b7c1ad8683 93 extern int CID_TX_VALVE_POSITION;
jeongyeseong 225:9c0becc196ba 94 extern int CID_TX_SOMETHING;
Lightvalve 19:23b7c1ad8683 95
hyhoon 227:699c3e572283 96 float temp_P_GAIN = 0.0f;
hyhoon 227:699c3e572283 97 float temp_I_GAIN = 0.0f;
hyhoon 227:699c3e572283 98 int temp_VELOCITY_COMP_GAIN = 0;
Lightvalve 235:c831718303c9 99 int logging = 0;
Lightvalve 257:c93d3eabff75 100 float valve_pos_pulse_can = 0.0f;
hyhoon 227:699c3e572283 101
Lightvalve 235:c831718303c9 102 inline float tanh_inv(float y)
Lightvalve 235:c831718303c9 103 {
Lightvalve 235:c831718303c9 104 if(y >= 1.0f - 0.000001f) y = 1.0f - 0.000001f;
Lightvalve 235:c831718303c9 105 if(y <= -1.0f + 0.000001f) y = -1.0f + 0.000001f;
jobuuu 230:2c3e5ecbe7e1 106 return log(sqrt((1.0f+y)/(1.0f-y)));
jobuuu 230:2c3e5ecbe7e1 107 }
jobuuu 230:2c3e5ecbe7e1 108
hyhoon 227:699c3e572283 109
Lightvalve 12:6f2531038ea4 110 /*******************************************************************************
Lightvalve 12:6f2531038ea4 111 * REFERENCE MODE
Lightvalve 12:6f2531038ea4 112 ******************************************************************************/
Lightvalve 13:747daba9cf59 113 enum _REFERENCE_MODE {
Lightvalve 235:c831718303c9 114 MODE_REF_NO_ACT = 0,
Lightvalve 235:c831718303c9 115 MODE_REF_DIRECT,
Lightvalve 235:c831718303c9 116 MODE_REF_FINDHOME
Lightvalve 12:6f2531038ea4 117 };
Lightvalve 12:6f2531038ea4 118
Lightvalve 12:6f2531038ea4 119 /*******************************************************************************
Lightvalve 12:6f2531038ea4 120 * CONTROL MODE
Lightvalve 12:6f2531038ea4 121 ******************************************************************************/
Lightvalve 13:747daba9cf59 122 enum _CONTROL_MODE {
Lightvalve 12:6f2531038ea4 123 //control mode
Lightvalve 12:6f2531038ea4 124 MODE_NO_ACT = 0, //0
Lightvalve 47:fdcb8bd86fd6 125 MODE_VALVE_POSITION_CONTROL, //1
Lightvalve 47:fdcb8bd86fd6 126 MODE_JOINT_CONTROL, //2
Lightvalve 50:3c630b5eba9f 127
Lightvalve 47:fdcb8bd86fd6 128 MODE_VALVE_OPEN_LOOP, //3
Lightvalve 138:a843f32ced33 129 MODE_JOINT_ADAPTIVE_BACKSTEPPING, //4
Lightvalve 171:bfc1fd2629d8 130 MODE_RL, //5
Lightvalve 13:747daba9cf59 131
Lightvalve 12:6f2531038ea4 132 MODE_JOINT_POSITION_PRES_CONTROL_PWM, //6
Lightvalve 12:6f2531038ea4 133 MODE_JOINT_POSITION_PRES_CONTROL_VALVE_POSITION, //7
Lightvalve 12:6f2531038ea4 134 MODE_VALVE_POSITION_PRES_CONTROL_LEARNING, //8
Lightvalve 13:747daba9cf59 135
Lightvalve 12:6f2531038ea4 136 MODE_TEST_CURRENT_CONTROL, //9
Lightvalve 12:6f2531038ea4 137 MODE_TEST_PWM_CONTROL, //10
Lightvalve 21:e5f1a43ea6f9 138
Lightvalve 14:8e7590227d22 139 MODE_CURRENT_CONTROL, //11
Lightvalve 14:8e7590227d22 140 MODE_JOINT_POSITION_TORQUE_CONTROL_CURRENT, //12
Lightvalve 14:8e7590227d22 141 MODE_JOINT_POSITION_PRES_CONTROL_CURRENT, //13
Lightvalve 224:985dba42f261 142 MODE_VALVE_POSITION_TORQUE_CONTROL_LEARNING, //14
Lightvalve 13:747daba9cf59 143
Lightvalve 12:6f2531038ea4 144 //utility
Lightvalve 12:6f2531038ea4 145 MODE_TORQUE_SENSOR_NULLING = 20, //20
Lightvalve 12:6f2531038ea4 146 MODE_VALVE_NULLING_AND_DEADZONE_SETTING, //21
Lightvalve 12:6f2531038ea4 147 MODE_FIND_HOME, //22
Lightvalve 57:f4819de54e7a 148 MODE_VALVE_GAIN_SETTING, //23
Lightvalve 12:6f2531038ea4 149 MODE_PRESSURE_SENSOR_NULLING, //24
Lightvalve 12:6f2531038ea4 150 MODE_PRESSURE_SENSOR_CALIB, //25
Lightvalve 12:6f2531038ea4 151 MODE_ROTARY_FRICTION_TUNING, //26
Lightvalve 13:747daba9cf59 152
Lightvalve 224:985dba42f261 153 MODE_DDV_POS_VS_PWM_ID = 30, //30
Lightvalve 12:6f2531038ea4 154 MODE_DDV_DEADZONE_AND_CENTER, //31
Lightvalve 12:6f2531038ea4 155 MODE_DDV_POS_VS_FLOWRATE, //32
Lightvalve 57:f4819de54e7a 156 MODE_SYSTEM_ID, //33
Lightvalve 169:645207e160ca 157 MODE_FREQ_TEST, //34
Lightvalve 169:645207e160ca 158 MODE_SEND_BUFFER, //35
Lightvalve 169:645207e160ca 159 MODE_SEND_OVER, //36
Lightvalve 169:645207e160ca 160 MODE_STEP_TEST, //37
Lightvalve 12:6f2531038ea4 161 };
Lightvalve 12:6f2531038ea4 162
Lightvalve 65:a2d7c63419c2 163 void SystemClock_Config(void)
Lightvalve 65:a2d7c63419c2 164 {
Lightvalve 169:645207e160ca 165 RCC_OscInitTypeDef RCC_OscInitStruct = {0};
Lightvalve 169:645207e160ca 166 RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};
Lightvalve 65:a2d7c63419c2 167
Lightvalve 224:985dba42f261 168 /* Configure the main internal regulator output voltage
Lightvalve 169:645207e160ca 169 */
Lightvalve 169:645207e160ca 170 __HAL_RCC_PWR_CLK_ENABLE();
Lightvalve 169:645207e160ca 171 __HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE1);
Lightvalve 224:985dba42f261 172 /* Initializes the CPU, AHB and APB busses clocks
Lightvalve 169:645207e160ca 173 */
Lightvalve 257:c93d3eabff75 174 RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSE;
Lightvalve 257:c93d3eabff75 175 RCC_OscInitStruct.HSEState = RCC_HSE_ON;
Lightvalve 257:c93d3eabff75 176 // RCC_OscInitStruct.HSICalibrationValue = RCC_HSICALIBRATION_DEFAULT;
Lightvalve 169:645207e160ca 177 RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
Lightvalve 257:c93d3eabff75 178 RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE;
Lightvalve 257:c93d3eabff75 179 RCC_OscInitStruct.PLL.PLLM = 1;//4
Lightvalve 257:c93d3eabff75 180 RCC_OscInitStruct.PLL.PLLN = 192; //96
Lightvalve 169:645207e160ca 181 RCC_OscInitStruct.PLL.PLLP = RCC_PLLP_DIV2;
Lightvalve 169:645207e160ca 182 RCC_OscInitStruct.PLL.PLLQ = 2;
Lightvalve 169:645207e160ca 183 RCC_OscInitStruct.PLL.PLLR = 2;
Lightvalve 169:645207e160ca 184 if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK) {
Lightvalve 169:645207e160ca 185 //Error_Handler();
Lightvalve 169:645207e160ca 186 }
Lightvalve 169:645207e160ca 187 /** Activate the Over-Drive mode
Lightvalve 169:645207e160ca 188 */
Lightvalve 169:645207e160ca 189 if (HAL_PWREx_EnableOverDrive() != HAL_OK) {
Lightvalve 169:645207e160ca 190 //Error_Handler();
Lightvalve 169:645207e160ca 191 }
Lightvalve 169:645207e160ca 192 /** Initializes the CPU, AHB and APB busses clocks
Lightvalve 169:645207e160ca 193 */
Lightvalve 169:645207e160ca 194 RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK
Lightvalve 169:645207e160ca 195 |RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2;
Lightvalve 169:645207e160ca 196 RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
Lightvalve 169:645207e160ca 197 RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
Lightvalve 169:645207e160ca 198 RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV4;
Lightvalve 257:c93d3eabff75 199 RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1;
Lightvalve 65:a2d7c63419c2 200
Lightvalve 169:645207e160ca 201 if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_5) != HAL_OK) {
Lightvalve 169:645207e160ca 202 //Error_Handler();
Lightvalve 169:645207e160ca 203 }
Lightvalve 257:c93d3eabff75 204 // HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_5);
Lightvalve 257:c93d3eabff75 205 // HAL_SYSTICK_Config(HAL_RCC_GetHCLKFreq()/1000);
Lightvalve 257:c93d3eabff75 206 // HAL_SYSTICK_CLKSourceConfig(SYSTICK_CLKSOURCE_HCLK);
Lightvalve 257:c93d3eabff75 207 // HAL_NVIC_SetPriority(SysTick_IRQn, 0, 0);
Lightvalve 257:c93d3eabff75 208
Lightvalve 65:a2d7c63419c2 209 }
Lightvalve 65:a2d7c63419c2 210
Lightvalve 170:42c938a40313 211
Lightvalve 257:c93d3eabff75 212 //void SystemClock_Config(void) //External clock
Lightvalve 257:c93d3eabff75 213 //{
Lightvalve 257:c93d3eabff75 214 // RCC_OscInitTypeDef RCC_OscInitStruct;
Lightvalve 257:c93d3eabff75 215 // RCC_ClkInitTypeDef RCC_ClkInitStruct;
Lightvalve 257:c93d3eabff75 216 //
Lightvalve 257:c93d3eabff75 217 // /* Configure the main internal regulator output voltage
Lightvalve 257:c93d3eabff75 218 // */
Lightvalve 257:c93d3eabff75 219 // __PWR_CLK_ENABLE();
Lightvalve 257:c93d3eabff75 220 // __HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE1);
Lightvalve 257:c93d3eabff75 221 // /* Initializes the CPU, AHB and APB busses clocks
Lightvalve 257:c93d3eabff75 222 // */
Lightvalve 257:c93d3eabff75 223 // RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSE;
Lightvalve 257:c93d3eabff75 224 // RCC_OscInitStruct.HSIState = RCC_HSE_ON;
Lightvalve 257:c93d3eabff75 225 //// RCC_OscInitStruct.HSICalibrationValue = RCC_HSICALIBRATION_DEFAULT;
Lightvalve 257:c93d3eabff75 226 // RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
Lightvalve 257:c93d3eabff75 227 // RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE;
Lightvalve 257:c93d3eabff75 228 // RCC_OscInitStruct.PLL.PLLM = 8;//8
Lightvalve 257:c93d3eabff75 229 // RCC_OscInitStruct.PLL.PLLN = 80; //180
Lightvalve 257:c93d3eabff75 230 // RCC_OscInitStruct.PLL.PLLP = RCC_PLLP_DIV2;
Lightvalve 257:c93d3eabff75 231 // RCC_OscInitStruct.PLL.PLLQ = 4;
Lightvalve 257:c93d3eabff75 232 // HAL_RCC_OscConfig(&RCC_OscInitStruct);
Lightvalve 257:c93d3eabff75 233 //
Lightvalve 257:c93d3eabff75 234 // HAL_PWREx_ActivateOverDrive();
Lightvalve 257:c93d3eabff75 235 //// RCC_OscInitStruct.PLL.PLLR = 2;
Lightvalve 257:c93d3eabff75 236 //// if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK) {
Lightvalve 257:c93d3eabff75 237 //// //Error_Handler();
Lightvalve 257:c93d3eabff75 238 //// }
Lightvalve 257:c93d3eabff75 239 //// /** Activate the Over-Drive mode
Lightvalve 257:c93d3eabff75 240 //// */
Lightvalve 257:c93d3eabff75 241 //// if (HAL_PWREx_EnableOverDrive() != HAL_OK) {
Lightvalve 257:c93d3eabff75 242 //// //Error_Handler();
Lightvalve 257:c93d3eabff75 243 //// }
Lightvalve 257:c93d3eabff75 244 // /** Initializes the CPU, AHB and APB busses clocks
Lightvalve 257:c93d3eabff75 245 // */
Lightvalve 257:c93d3eabff75 246 // RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_SYSCLK
Lightvalve 257:c93d3eabff75 247 // |RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2;
Lightvalve 257:c93d3eabff75 248 // RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
Lightvalve 257:c93d3eabff75 249 // RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
Lightvalve 257:c93d3eabff75 250 // RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV4;
Lightvalve 257:c93d3eabff75 251 // RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV2;
Lightvalve 257:c93d3eabff75 252 // HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_5);
Lightvalve 257:c93d3eabff75 253 //
Lightvalve 257:c93d3eabff75 254 // HAL_SYSTICK_Config(HAL_RCC_GetHCLKFreq()/1000);
Lightvalve 257:c93d3eabff75 255 //
Lightvalve 257:c93d3eabff75 256 // HAL_SYSTICK_CLKSourceConfig(SYSTICK_CLKSOURCE_HCLK);
Lightvalve 257:c93d3eabff75 257 //
Lightvalve 257:c93d3eabff75 258 // HAL_NVIC_SetPriority(SysTick_IRQn, 0, 0);
Lightvalve 257:c93d3eabff75 259 //}
Lightvalve 257:c93d3eabff75 260
GiJeongKim 0:51c43836c1d7 261 int main()
GiJeongKim 0:51c43836c1d7 262 {
jobuuu 6:df07d3491e3a 263 /*********************************
jobuuu 1:e04e563be5ce 264 *** Initialization
jobuuu 6:df07d3491e3a 265 *********************************/
hyhoon 226:82a3ca333004 266
Lightvalve 224:985dba42f261 267 HAL_Init();
Lightvalve 224:985dba42f261 268 SystemClock_Config();
hyhoon 226:82a3ca333004 269
Lightvalve 69:3995ffeaa786 270 LED = 0;
Lightvalve 257:c93d3eabff75 271 // pc.baud(9600);
Lightvalve 257:c93d3eabff75 272 //
Lightvalve 257:c93d3eabff75 273 // // i2c init
Lightvalve 257:c93d3eabff75 274 // i2c.frequency(400 * 1000); // 0.4 mHz
Lightvalve 257:c93d3eabff75 275 // wait_ms(2); // Power Up wait
Lightvalve 257:c93d3eabff75 276 // look_for_hardware_i2c(); // Hardware present
Lightvalve 257:c93d3eabff75 277 // init_as5510(i2c_slave_addr1);
Lightvalve 257:c93d3eabff75 278 // make_delay();
Lightvalve 21:e5f1a43ea6f9 279
jobuuu 2:a1c0a37df760 280
Lightvalve 224:985dba42f261 281 // spi init
Lightvalve 257:c93d3eabff75 282 // eeprom_cs = 1;
Lightvalve 257:c93d3eabff75 283 // eeprom.format(8,3);
Lightvalve 257:c93d3eabff75 284 // eeprom.frequency(5000000); //5M
Lightvalve 257:c93d3eabff75 285 // eeprom_cs = 0;
Lightvalve 257:c93d3eabff75 286 // make_delay();
Lightvalve 257:c93d3eabff75 287 //
Lightvalve 257:c93d3eabff75 288 // enc_cs = 1; //sw add
Lightvalve 257:c93d3eabff75 289 // enc.format(8,0);
Lightvalve 257:c93d3eabff75 290 // enc.frequency(5000000); //10M
Lightvalve 257:c93d3eabff75 291 // enc_cs = 0; //sw add
Lightvalve 257:c93d3eabff75 292 //
Lightvalve 257:c93d3eabff75 293 // make_delay();
Lightvalve 257:c93d3eabff75 294 //
Lightvalve 257:c93d3eabff75 295 // // spi _ enc
Lightvalve 257:c93d3eabff75 296 // spi_enc_set_init();
Lightvalve 257:c93d3eabff75 297 // make_delay();
Lightvalve 257:c93d3eabff75 298 //
Lightvalve 257:c93d3eabff75 299 // ////// bno rom
Lightvalve 257:c93d3eabff75 300 // spi_eeprom_write(RID_BNO, (int16_t) 0);
Lightvalve 257:c93d3eabff75 301 // make_delay();
hyhoon 226:82a3ca333004 302 ////////
hyhoon 226:82a3ca333004 303
Lightvalve 224:985dba42f261 304 // rom
Lightvalve 19:23b7c1ad8683 305 ROM_CALL_DATA();
Lightvalve 16:903b5a4433b4 306 make_delay();
Lightvalve 13:747daba9cf59 307
GiJeongKim 0:51c43836c1d7 308 // ADC init
jobuuu 5:a4319f79457b 309 Init_ADC();
Lightvalve 11:82d8768d7351 310 make_delay();
jobuuu 2:a1c0a37df760 311
GiJeongKim 0:51c43836c1d7 312 // Pwm init
GiJeongKim 0:51c43836c1d7 313 Init_PWM();
GiJeongKim 0:51c43836c1d7 314 TIM4->CR1 ^= TIM_CR1_UDIS;
Lightvalve 11:82d8768d7351 315 make_delay();
Lightvalve 13:747daba9cf59 316
Lightvalve 257:c93d3eabff75 317 // // CAN
Lightvalve 257:c93d3eabff75 318 // can.attach(&CAN_RX_HANDLER);
Lightvalve 11:82d8768d7351 319 CAN_ID_INIT();
Lightvalve 11:82d8768d7351 320 make_delay();
Lightvalve 34:bb2ca2fc2a8e 321
hyhoon 226:82a3ca333004 322 //can.reset();
Lightvalve 257:c93d3eabff75 323 // can.filter(msg.id, 0xFFFFF000, CANStandard);
Lightvalve 257:c93d3eabff75 324
Lightvalve 257:c93d3eabff75 325 // // TMR2 init
Lightvalve 257:c93d3eabff75 326 // Init_TMR2();
Lightvalve 257:c93d3eabff75 327 // TIM2->CR1 ^= TIM_CR1_UDIS;
Lightvalve 257:c93d3eabff75 328 // make_delay();
hyhoon 226:82a3ca333004 329
hyhoon 226:82a3ca333004 330 // TMR3 init
hyhoon 226:82a3ca333004 331 Init_TMR3();
hyhoon 226:82a3ca333004 332 TIM3->CR1 ^= TIM_CR1_UDIS;
hyhoon 226:82a3ca333004 333 make_delay();
hyhoon 226:82a3ca333004 334
Lightvalve 257:c93d3eabff75 335 // //Timer priority
Lightvalve 257:c93d3eabff75 336 NVIC_SetPriority(TIM3_IRQn, 3);
Lightvalve 257:c93d3eabff75 337 NVIC_SetPriority(TIM4_IRQn, 2);
Lightvalve 257:c93d3eabff75 338 // NVIC_SetPriority(TIM2_IRQn, 2);
Lightvalve 257:c93d3eabff75 339 //
Lightvalve 257:c93d3eabff75 340 //
Lightvalve 257:c93d3eabff75 341 // //DAC init
Lightvalve 257:c93d3eabff75 342 // if (SENSING_MODE == 0) {
Lightvalve 257:c93d3eabff75 343 // dac_1 = FORCE_VREF / 3.3f;
Lightvalve 257:c93d3eabff75 344 // dac_2 = 0.0f;
Lightvalve 257:c93d3eabff75 345 // } else if (SENSING_MODE == 1) {
Lightvalve 257:c93d3eabff75 346 // // if (DIR_VALVE_ENC > 0) {
Lightvalve 257:c93d3eabff75 347 // dac_1 = PRES_A_VREF / 3.3f;
Lightvalve 257:c93d3eabff75 348 // dac_2 = PRES_B_VREF / 3.3f;
Lightvalve 257:c93d3eabff75 349 // // } else {
Lightvalve 257:c93d3eabff75 350 // // dac_1 = PRES_B_VREF / 3.3f;
Lightvalve 257:c93d3eabff75 351 // // dac_2 = PRES_A_VREF / 3.3f;
Lightvalve 257:c93d3eabff75 352 // // }
Lightvalve 257:c93d3eabff75 353 // }
Lightvalve 257:c93d3eabff75 354 // make_delay();
Lightvalve 34:bb2ca2fc2a8e 355
hyhoon 226:82a3ca333004 356
Lightvalve 13:747daba9cf59 357
Lightvalve 257:c93d3eabff75 358 TIM4->CCR2 = (PWM_ARR)*(1.0f-0.0f);
Lightvalve 257:c93d3eabff75 359 TIM4->CCR1 = (PWM_ARR)*(1.0f-0.0f);
Lightvalve 257:c93d3eabff75 360
Lightvalve 169:645207e160ca 361
jobuuu 6:df07d3491e3a 362 /************************************
jobuuu 1:e04e563be5ce 363 *** Program is operating!
jobuuu 6:df07d3491e3a 364 *************************************/
GiJeongKim 0:51c43836c1d7 365 while(1) {
Lightvalve 169:645207e160ca 366
Lightvalve 257:c93d3eabff75 367 // if (LED > 0) LED = 0;
Lightvalve 257:c93d3eabff75 368 // else LED = 1;
Lightvalve 257:c93d3eabff75 369 // ADC1->CR2 |= 0x40000000;
Lightvalve 257:c93d3eabff75 370 // LVDT_new = ((float)ADC1->DR) - 2047.5f;
Lightvalve 257:c93d3eabff75 371 // TIM4->CCR2 = (PWM_ARR)*(1.0f-0.0f);
Lightvalve 257:c93d3eabff75 372 // TIM4->CCR1 = (PWM_ARR)*(1.0f-0.3f);
GiJeongKim 0:51c43836c1d7 373 }
jobuuu 1:e04e563be5ce 374 }
jobuuu 1:e04e563be5ce 375
Lightvalve 224:985dba42f261 376
jobuuu 230:2c3e5ecbe7e1 377 //------------------------------------------------
jobuuu 230:2c3e5ecbe7e1 378 // TMR4 : Sensor Read & Data Handling
jobuuu 230:2c3e5ecbe7e1 379 //-----------------------------------------------
Lightvalve 51:b46bed7fec80 380 float FREQ_TMR4 = (float)FREQ_20k;
Lightvalve 51:b46bed7fec80 381 float DT_TMR4 = (float)DT_20k;
Lightvalve 57:f4819de54e7a 382 long CNT_TMR4 = 0;
Lightvalve 257:c93d3eabff75 383 int TMR4_FREQ_20k = (int)FREQ_20k;
Lightvalve 257:c93d3eabff75 384 //int toggle = 0;
Lightvalve 257:c93d3eabff75 385 int PWM_Flag = 0;
Lightvalve 257:c93d3eabff75 386 int TMR4_timer = 0;
Lightvalve 257:c93d3eabff75 387
Lightvalve 257:c93d3eabff75 388 float LVDT_new = 0.0f;
Lightvalve 257:c93d3eabff75 389 float LVDT_old = 0.0f;
Lightvalve 257:c93d3eabff75 390 float LVDT_f_cut = 1000.0f;
Lightvalve 257:c93d3eabff75 391 float LVDT_LPF = 0.0f;
Lightvalve 257:c93d3eabff75 392 float LVDT_sum = 0.0f;
Lightvalve 257:c93d3eabff75 393
jobuuu 1:e04e563be5ce 394 extern "C" void TIM4_IRQHandler(void)
jobuuu 1:e04e563be5ce 395 {
Lightvalve 19:23b7c1ad8683 396 if (TIM4->SR & TIM_SR_UIF ) {
Lightvalve 13:747daba9cf59 397
Lightvalve 257:c93d3eabff75 398 // if (LED > 0) LED = 0;
Lightvalve 257:c93d3eabff75 399 // else LED = 1;
jobuuu 230:2c3e5ecbe7e1 400
Lightvalve 257:c93d3eabff75 401 PWM_Flag++;
Lightvalve 257:c93d3eabff75 402
Lightvalve 257:c93d3eabff75 403 if(PWM_Flag <= 15) {
Lightvalve 61:bc8c8270f0ab 404
Lightvalve 257:c93d3eabff75 405 if (PWM_Flag >= 4 && PWM_Flag <=13) {
Lightvalve 257:c93d3eabff75 406 ADC1->CR2 |= 0x40000000;
Lightvalve 257:c93d3eabff75 407 LVDT_new = ((float)ADC1->DR) - 2047.5f;
Lightvalve 257:c93d3eabff75 408 // if (ADC1->SR &= ~(ADC_SR_EOC)) {
Lightvalve 257:c93d3eabff75 409 // if (LED > 0) LED = 0;
Lightvalve 257:c93d3eabff75 410 // else LED = 1;
Lightvalve 257:c93d3eabff75 411 // LED = 1;
Lightvalve 257:c93d3eabff75 412 // }
Lightvalve 257:c93d3eabff75 413 LVDT_sum = LVDT_sum + LVDT_new;
Lightvalve 257:c93d3eabff75 414 if (LED > 0) LED = 0;
Lightvalve 257:c93d3eabff75 415 else LED = 1;
Lightvalve 257:c93d3eabff75 416 }
Lightvalve 50:3c630b5eba9f 417
Lightvalve 257:c93d3eabff75 418 //pwm
Lightvalve 257:c93d3eabff75 419 TIM4->CCR2 = (PWM_ARR)*(1.0f-0.0f);
Lightvalve 257:c93d3eabff75 420 TIM4->CCR1 = (PWM_ARR)*(1.0f-1.0f);
Lightvalve 257:c93d3eabff75 421 } else if(PWM_Flag <= 500) {
Lightvalve 257:c93d3eabff75 422 TIM4->CCR2 = (PWM_ARR)*(1.0f-0.0f);
Lightvalve 257:c93d3eabff75 423 TIM4->CCR1 = (PWM_ARR)*(1.0f-0.0f);
Lightvalve 257:c93d3eabff75 424 } else {
Lightvalve 257:c93d3eabff75 425 PWM_Flag = 0;
Lightvalve 257:c93d3eabff75 426 LVDT_LPF = 0.0f;
Lightvalve 257:c93d3eabff75 427 force.sen = LVDT_sum * 0.1f;
Lightvalve 257:c93d3eabff75 428 LVDT_sum = 0.0f;
Lightvalve 58:2eade98630e2 429 }
Lightvalve 61:bc8c8270f0ab 430
Lightvalve 57:f4819de54e7a 431 CNT_TMR4++;
Lightvalve 21:e5f1a43ea6f9 432 }
Lightvalve 11:82d8768d7351 433 TIM4->SR = 0x0; // reset the status register
Lightvalve 11:82d8768d7351 434 }
Lightvalve 19:23b7c1ad8683 435
Lightvalve 19:23b7c1ad8683 436
Lightvalve 18:b8adf1582ea3 437 int j =0;
Lightvalve 54:647072f5307a 438 float FREQ_TMR3 = (float)FREQ_5k;
Lightvalve 48:889798ff9329 439 float DT_TMR3 = (float)DT_5k;
Lightvalve 45:35fa6884d0c6 440 int cnt_trans = 0;
Lightvalve 48:889798ff9329 441 double VALVE_POS_RAW_FORCE_FB_LOGGING = 0.0f;
Lightvalve 57:f4819de54e7a 442 int can_rest =0;
Lightvalve 239:8ac5c6162bc1 443 float force_ref_act_can = 0.0f;
Lightvalve 48:889798ff9329 444
Lightvalve 11:82d8768d7351 445 extern "C" void TIM3_IRQHandler(void)
Lightvalve 21:e5f1a43ea6f9 446 {
Lightvalve 19:23b7c1ad8683 447 if (TIM3->SR & TIM_SR_UIF ) {
Lightvalve 57:f4819de54e7a 448
Lightvalve 45:35fa6884d0c6 449
Lightvalve 50:3c630b5eba9f 450
Lightvalve 257:c93d3eabff75 451 // // =====================================================================
Lightvalve 257:c93d3eabff75 452 // // CONTROL LOOP --------------------------------------------------------
Lightvalve 257:c93d3eabff75 453 // // =====================================================================
Lightvalve 257:c93d3eabff75 454 // int UTILITY_MODE = 0;
Lightvalve 257:c93d3eabff75 455 // int CONTROL_MODE = 0;
Lightvalve 257:c93d3eabff75 456 //
Lightvalve 257:c93d3eabff75 457 // if (CONTROL_UTILITY_MODE >= 20 || CONTROL_UTILITY_MODE == 0) {
Lightvalve 257:c93d3eabff75 458 // UTILITY_MODE = CONTROL_UTILITY_MODE;
Lightvalve 257:c93d3eabff75 459 // CONTROL_MODE = MODE_NO_ACT;
Lightvalve 257:c93d3eabff75 460 // } else {
Lightvalve 257:c93d3eabff75 461 // CONTROL_MODE = CONTROL_UTILITY_MODE;
Lightvalve 257:c93d3eabff75 462 // UTILITY_MODE = MODE_NO_ACT;
Lightvalve 257:c93d3eabff75 463 // }
Lightvalve 257:c93d3eabff75 464 // // UTILITY MODE ------------------------------------------------------------
Lightvalve 257:c93d3eabff75 465 // switch (UTILITY_MODE) {
Lightvalve 257:c93d3eabff75 466 // case MODE_NO_ACT: {
Lightvalve 257:c93d3eabff75 467 // break;
Lightvalve 257:c93d3eabff75 468 // }
Lightvalve 257:c93d3eabff75 469 //
Lightvalve 257:c93d3eabff75 470 // case MODE_TORQUE_SENSOR_NULLING: {
Lightvalve 257:c93d3eabff75 471 // static float FORCE_pulse_sum = 0.0;
Lightvalve 257:c93d3eabff75 472 // static float PresA_pulse_sum = 0.0;
Lightvalve 257:c93d3eabff75 473 // static float PresB_pulse_sum = 0.0;
Lightvalve 257:c93d3eabff75 474 //
Lightvalve 257:c93d3eabff75 475 // // DAC Voltage reference set
Lightvalve 257:c93d3eabff75 476 // float VREF_TuningGain = -0.00000003f;
Lightvalve 257:c93d3eabff75 477 // if (TMR3_COUNT_TORQUE_NULL < TMR_FREQ_5k * 5) {
Lightvalve 257:c93d3eabff75 478 // if(SENSING_MODE == 0) { // Force Sensor (Loadcell)
Lightvalve 257:c93d3eabff75 479 // FORCE_pulse_sum = FORCE_pulse_sum + force.sen*TORQUE_SENSOR_PULSE_PER_TORQUE;
Lightvalve 257:c93d3eabff75 480 // if (TMR3_COUNT_TORQUE_NULL % 10 == 0) {
Lightvalve 257:c93d3eabff75 481 // float FORCE_pluse_mean = FORCE_pulse_sum / 10.0f;
Lightvalve 257:c93d3eabff75 482 // FORCE_pulse_sum = 0.0f;
Lightvalve 257:c93d3eabff75 483 //
Lightvalve 257:c93d3eabff75 484 // FORCE_VREF += VREF_TuningGain * (0.0f - FORCE_pluse_mean);
Lightvalve 257:c93d3eabff75 485 // if (FORCE_VREF > 3.3f) FORCE_VREF = 3.3f;
Lightvalve 257:c93d3eabff75 486 // if (FORCE_VREF < 0.0f) FORCE_VREF = 0.0f;
Lightvalve 257:c93d3eabff75 487 // dac_1 = FORCE_VREF / 3.3f;
Lightvalve 257:c93d3eabff75 488 // }
Lightvalve 257:c93d3eabff75 489 // } else if (SENSING_MODE == 1) { // Pressure Sensor
Lightvalve 257:c93d3eabff75 490 // PresA_pulse_sum += pres_A.sen*PRES_SENSOR_A_PULSE_PER_BAR;
Lightvalve 257:c93d3eabff75 491 // PresB_pulse_sum += pres_B.sen*PRES_SENSOR_B_PULSE_PER_BAR;
Lightvalve 257:c93d3eabff75 492 // if (TMR3_COUNT_TORQUE_NULL % 10 == 0) {
Lightvalve 257:c93d3eabff75 493 // float PresA_pluse_mean = PresA_pulse_sum / 10.0f;
Lightvalve 257:c93d3eabff75 494 // float PresB_pluse_mean = PresB_pulse_sum / 10.0f;
Lightvalve 257:c93d3eabff75 495 // PresA_pulse_sum = 0.0f;
Lightvalve 257:c93d3eabff75 496 // PresB_pulse_sum = 0.0f;
Lightvalve 257:c93d3eabff75 497 //
Lightvalve 257:c93d3eabff75 498 // PRES_A_VREF += VREF_TuningGain * (0.0f - PresA_pluse_mean);
Lightvalve 257:c93d3eabff75 499 // if (PRES_A_VREF > 3.3f) PRES_A_VREF = 3.3f;
Lightvalve 257:c93d3eabff75 500 // if (PRES_A_VREF < 0.0f) PRES_A_VREF = 0.0f;
Lightvalve 257:c93d3eabff75 501 // dac_1 = PRES_A_VREF / 3.3f;
Lightvalve 257:c93d3eabff75 502 // PRES_B_VREF += VREF_TuningGain * (0.0f - PresB_pluse_mean);
Lightvalve 257:c93d3eabff75 503 // if (PRES_B_VREF > 3.3f) PRES_B_VREF = 3.3f;
Lightvalve 257:c93d3eabff75 504 // if (PRES_B_VREF < 0.0f) PRES_B_VREF = 0.0f;
Lightvalve 257:c93d3eabff75 505 // dac_2 = PRES_B_VREF / 3.3f;
Lightvalve 257:c93d3eabff75 506 // }
Lightvalve 257:c93d3eabff75 507 // }
Lightvalve 257:c93d3eabff75 508 // TMR3_COUNT_TORQUE_NULL++;
Lightvalve 257:c93d3eabff75 509 // } else {
Lightvalve 257:c93d3eabff75 510 // if(SENSING_MODE == 0 ) { // Force Sensor (Loadcell)
Lightvalve 257:c93d3eabff75 511 // FORCE_pulse_sum = 0.0f;
Lightvalve 257:c93d3eabff75 512 // dac_1 = FORCE_VREF / 3.3f;
Lightvalve 257:c93d3eabff75 513 // spi_eeprom_write(RID_FORCE_SENSOR_VREF, (int16_t)(FORCE_VREF * 1000.0f));
Lightvalve 257:c93d3eabff75 514 // } else if (SENSING_MODE == 1) {
Lightvalve 257:c93d3eabff75 515 // PresA_pulse_sum = 0.0f;
Lightvalve 257:c93d3eabff75 516 // PresB_pulse_sum = 0.0f;
Lightvalve 257:c93d3eabff75 517 // dac_1 = PRES_A_VREF / 3.3f;
Lightvalve 257:c93d3eabff75 518 // dac_2 = PRES_B_VREF / 3.3f;
Lightvalve 257:c93d3eabff75 519 // spi_eeprom_write(RID_PRES_A_SENSOR_VREF, (int16_t)(PRES_A_VREF * 1000.0f));
Lightvalve 257:c93d3eabff75 520 // spi_eeprom_write(RID_PRES_B_SENSOR_VREF, (int16_t)(PRES_B_VREF * 1000.0f));
Lightvalve 257:c93d3eabff75 521 // }
Lightvalve 257:c93d3eabff75 522 // CONTROL_UTILITY_MODE = MODE_NO_ACT;
Lightvalve 257:c93d3eabff75 523 // TMR3_COUNT_TORQUE_NULL = 0;
Lightvalve 257:c93d3eabff75 524 // }
Lightvalve 257:c93d3eabff75 525 // break;
Lightvalve 257:c93d3eabff75 526 // }
Lightvalve 257:c93d3eabff75 527 //
Lightvalve 257:c93d3eabff75 528 //
Lightvalve 257:c93d3eabff75 529 // default:
Lightvalve 257:c93d3eabff75 530 // break;
Lightvalve 257:c93d3eabff75 531 // }
Lightvalve 257:c93d3eabff75 532 //
Lightvalve 257:c93d3eabff75 533 // // CONTROL MODE ------------------------------------------------------------
Lightvalve 257:c93d3eabff75 534 // switch (CONTROL_MODE) {
Lightvalve 257:c93d3eabff75 535 // case MODE_NO_ACT: {
Lightvalve 257:c93d3eabff75 536 // V_out = 0.0f;
Lightvalve 257:c93d3eabff75 537 // break;
Lightvalve 257:c93d3eabff75 538 // }
Lightvalve 257:c93d3eabff75 539 //
Lightvalve 257:c93d3eabff75 540 // case MODE_VALVE_OPEN_LOOP: {
Lightvalve 257:c93d3eabff75 541 // V_out = (float) Vout.ref;
Lightvalve 257:c93d3eabff75 542 // break;
Lightvalve 257:c93d3eabff75 543 // }
Lightvalve 257:c93d3eabff75 544 //
Lightvalve 257:c93d3eabff75 545 // default:
Lightvalve 257:c93d3eabff75 546 // break;
Lightvalve 257:c93d3eabff75 547 // }
Lightvalve 257:c93d3eabff75 548 //
Lightvalve 257:c93d3eabff75 549 // if (V_out > 0 ) V_out = (V_out + 180.0f)/0.8588f;
Lightvalve 257:c93d3eabff75 550 // else if (V_out < 0) V_out = (V_out - 200.0f)/0.8651f;
Lightvalve 257:c93d3eabff75 551 // else V_out = 0.0f;
Lightvalve 257:c93d3eabff75 552 //
Lightvalve 257:c93d3eabff75 553 //
Lightvalve 257:c93d3eabff75 554 // ////////////////////////////////////////////////////////////////////
Lightvalve 257:c93d3eabff75 555 // /////////////////// PWM Command ///////////////////////////////////
Lightvalve 257:c93d3eabff75 556 // ////////////////////////////////////////////////////////////////////
Lightvalve 257:c93d3eabff75 557 //
Lightvalve 257:c93d3eabff75 558 // if(DIR_VALVE<0) {
Lightvalve 257:c93d3eabff75 559 // V_out = -V_out;
Lightvalve 257:c93d3eabff75 560 // }
Lightvalve 257:c93d3eabff75 561 //
Lightvalve 257:c93d3eabff75 562 // if (V_out >= VALVE_VOLTAGE_LIMIT*1000.0f) {
Lightvalve 257:c93d3eabff75 563 // V_out = VALVE_VOLTAGE_LIMIT*1000.0f;
Lightvalve 257:c93d3eabff75 564 // } else if(V_out<=-VALVE_VOLTAGE_LIMIT*1000.0f) {
Lightvalve 257:c93d3eabff75 565 // V_out = -VALVE_VOLTAGE_LIMIT*1000.0f;
Lightvalve 257:c93d3eabff75 566 // }
Lightvalve 257:c93d3eabff75 567 // PWM_out= V_out/(SUPPLY_VOLTAGE*1000.0f);
Lightvalve 257:c93d3eabff75 568 //
Lightvalve 257:c93d3eabff75 569 // // Saturation of output voltage
Lightvalve 257:c93d3eabff75 570 // if(PWM_out > 1.0f) PWM_out=1.0f;
Lightvalve 257:c93d3eabff75 571 // else if (PWM_out < -1.0f) PWM_out=-1.0f;
Lightvalve 257:c93d3eabff75 572 //
Lightvalve 257:c93d3eabff75 573 // if (PWM_out>0.0f) {
Lightvalve 257:c93d3eabff75 574 // dtc_v=0.0f;
Lightvalve 257:c93d3eabff75 575 // dtc_w=PWM_out;
Lightvalve 257:c93d3eabff75 576 // } else {
Lightvalve 257:c93d3eabff75 577 // dtc_v=-PWM_out;
Lightvalve 257:c93d3eabff75 578 // dtc_w=0.0f;
Lightvalve 257:c93d3eabff75 579 // }
Lightvalve 257:c93d3eabff75 580 //
Lightvalve 257:c93d3eabff75 581 //// //pwm
Lightvalve 257:c93d3eabff75 582 //// TIM4->CCR2 = (PWM_ARR)*(1.0f-dtc_v);
Lightvalve 257:c93d3eabff75 583 //// TIM4->CCR1 = (PWM_ARR)*(1.0f-dtc_w);
Lightvalve 257:c93d3eabff75 584 //
Lightvalve 257:c93d3eabff75 585 // ////////////////////////////////////////////////////////////////////////////
Lightvalve 257:c93d3eabff75 586 // ////////////////////// Data transmission through CAN //////////////////////
Lightvalve 257:c93d3eabff75 587 // ////////////////////////////////////////////////////////////////////////////
Lightvalve 257:c93d3eabff75 588 //
Lightvalve 257:c93d3eabff75 589 // if (TMR2_COUNT_CAN_TX % (int) ((int) TMR_FREQ_5k/CAN_FREQ) == 0) {
Lightvalve 257:c93d3eabff75 590
Lightvalve 257:c93d3eabff75 591 // Position, Velocity, and Torque (ID:1200)
Lightvalve 257:c93d3eabff75 592 if (flag_data_request[0] == LOW) {
Lightvalve 257:c93d3eabff75 593
Lightvalve 257:c93d3eabff75 594 if ((OPERATING_MODE & 0b01) == 0) { // Rotary Actuator
Lightvalve 257:c93d3eabff75 595 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));
Lightvalve 257:c93d3eabff75 596 // 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));
Lightvalve 257:c93d3eabff75 597
Lightvalve 257:c93d3eabff75 598 } else if ((OPERATING_MODE & 0b01) == 1) { // Linear Actuator
Lightvalve 257:c93d3eabff75 599 CAN_TX_POSITION_FT((int16_t) (pos.sen*200.0f), (int16_t) (vel.sen*20.0f), (int16_t) (force.sen));
Lightvalve 257:c93d3eabff75 600 // CAN_TX_POSITION_FT((int16_t) (pos.sen*200.0f), (int16_t) (valve_pos_can*20.0f), (int16_t) (force.sen*TORQUE_SENSOR_PULSE_PER_TORQUE*10.0f));
jobuuu 230:2c3e5ecbe7e1 601 }
jobuuu 230:2c3e5ecbe7e1 602 }
Lightvalve 235:c831718303c9 603
Lightvalve 257:c93d3eabff75 604 // Valve Position (ID:1300)
Lightvalve 257:c93d3eabff75 605 if (flag_data_request[1] == HIGH) {
Lightvalve 257:c93d3eabff75 606 CAN_TX_PWM((int16_t)(cur.sen/mA_PER_pulse));
Lightvalve 257:c93d3eabff75 607 // CAN_TX_PWM((int16_t)(TORQUE_SENSOR_PULSE_PER_TORQUE*10000.0f));
Lightvalve 57:f4819de54e7a 608 }
jobuuu 230:2c3e5ecbe7e1 609
Lightvalve 257:c93d3eabff75 610 // Others : Pressure A, B, Supply Pressure, etc. (for Debugging) (ID:1400)
Lightvalve 257:c93d3eabff75 611 if (flag_data_request[2] == HIGH) {
Lightvalve 257:c93d3eabff75 612 float valve_pos_can = 0.0f;
Lightvalve 257:c93d3eabff75 613 if(value >= VALVE_ELECTRIC_CENTER) {
Lightvalve 257:c93d3eabff75 614 valve_pos_can = 10000.0f*((float)value-(float)VALVE_ELECTRIC_CENTER)/((float)VALVE_MAX_POS-(float)VALVE_ELECTRIC_CENTER);
Lightvalve 257:c93d3eabff75 615 } else {
Lightvalve 257:c93d3eabff75 616 valve_pos_can = -10000.0f*((float)value -(float)VALVE_ELECTRIC_CENTER)/((float)VALVE_MIN_POS-(float)VALVE_ELECTRIC_CENTER);
Lightvalve 237:c2cfe349f5c1 617 }
Lightvalve 257:c93d3eabff75 618 float valve_pos_ref_can = 0.0f;
Lightvalve 257:c93d3eabff75 619 if(valve_pos.ref >= VALVE_ELECTRIC_CENTER) {
Lightvalve 257:c93d3eabff75 620 valve_pos_ref_can = 10000.0f*((float)valve_pos.ref-(float)VALVE_ELECTRIC_CENTER)/((float)VALVE_MAX_POS-(float)VALVE_ELECTRIC_CENTER);
Lightvalve 257:c93d3eabff75 621 } else {
Lightvalve 257:c93d3eabff75 622 valve_pos_ref_can = -10000.0f*((float)valve_pos.ref -(float)VALVE_ELECTRIC_CENTER)/((float)VALVE_MIN_POS-(float)VALVE_ELECTRIC_CENTER);
Lightvalve 237:c2cfe349f5c1 623 }
Lightvalve 238:4e660aa77eb7 624
Lightvalve 257:c93d3eabff75 625 valve_pos_ref_can = (float)valve_pos.ref;
Lightvalve 237:c2cfe349f5c1 626
Lightvalve 257:c93d3eabff75 627 CAN_TX_CURRENT((int16_t) valve_pos_can, (int16_t) valve_pos_pulse_can);
Lightvalve 57:f4819de54e7a 628 }
Lightvalve 57:f4819de54e7a 629
Lightvalve 257:c93d3eabff75 630 TMR2_COUNT_CAN_TX = 0;
Lightvalve 257:c93d3eabff75 631 // }
Lightvalve 54:647072f5307a 632 TMR2_COUNT_CAN_TX++;
Lightvalve 52:8ea76864368a 633
Lightvalve 20:806196fda269 634 }
Lightvalve 52:8ea76864368a 635 TIM3->SR = 0x0; // reset the status register
Lightvalve 52:8ea76864368a 636
Lightvalve 257:c93d3eabff75 637 }
Lightvalve 257:c93d3eabff75 638
Lightvalve 257:c93d3eabff75 639 //
Lightvalve 257:c93d3eabff75 640 //float FREQ_TMR2 = (float)FREQ_400k;
Lightvalve 257:c93d3eabff75 641 //float DT_TMR2 = (float)DT_400k;
Lightvalve 257:c93d3eabff75 642 //int TMR2_timer = 0;
Lightvalve 257:c93d3eabff75 643 //int toggle_old = 0;
Lightvalve 257:c93d3eabff75 644 //
Lightvalve 257:c93d3eabff75 645 //float LVDT_new = 0.0f;
Lightvalve 257:c93d3eabff75 646 //float LVDT_old = 0.0f;
Lightvalve 257:c93d3eabff75 647 //float LVDT_f_cut = 1000.0f;
Lightvalve 257:c93d3eabff75 648 //float LVDT_LPF = 0.0f;
Lightvalve 257:c93d3eabff75 649 //float LVDT_sum = 0.0f;
Lightvalve 257:c93d3eabff75 650 //
Lightvalve 257:c93d3eabff75 651 //extern "C" void TIM2_IRQHandler(void)
Lightvalve 257:c93d3eabff75 652 //{
Lightvalve 257:c93d3eabff75 653 // if (TIM2->SR & TIM_SR_UIF ) {
Lightvalve 257:c93d3eabff75 654 //// if (LED > 0) LED = 0;
Lightvalve 257:c93d3eabff75 655 //// else LED = 1;
Lightvalve 257:c93d3eabff75 656 //// LED = 1;
Lightvalve 257:c93d3eabff75 657 //
Lightvalve 257:c93d3eabff75 658 // if (toggle != toggle_old) {
Lightvalve 257:c93d3eabff75 659 // TMR2_timer = 0;
Lightvalve 257:c93d3eabff75 660 // LVDT_LPF = 0.0f;
Lightvalve 257:c93d3eabff75 661 // LVDT_sum = 0.0f;
Lightvalve 257:c93d3eabff75 662 //// if (LED > 0) LED = 0;
Lightvalve 257:c93d3eabff75 663 //// else LED = 1;
Lightvalve 257:c93d3eabff75 664 // }
Lightvalve 257:c93d3eabff75 665 //
Lightvalve 257:c93d3eabff75 666 // if (TMR2_timer >= 6 && TMR2_timer <=45) {
Lightvalve 257:c93d3eabff75 667 //// if (TMR2_timer >= 6 && TMR2_timer <=25) {
Lightvalve 257:c93d3eabff75 668 //// if (LED > 0) LED = 0;
Lightvalve 257:c93d3eabff75 669 //// else LED = 1;
Lightvalve 257:c93d3eabff75 670 //// LED = 0;
Lightvalve 257:c93d3eabff75 671 // ADC1->CR2 |= 0x40000000;
Lightvalve 257:c93d3eabff75 672 // LVDT_new = ((float)ADC1->DR) - 2047.5f;
Lightvalve 257:c93d3eabff75 673 // if (ADC1->SR &= ~(ADC_SR_EOC)) {
Lightvalve 257:c93d3eabff75 674 //// if (LED > 0) LED = 0;
Lightvalve 257:c93d3eabff75 675 //// else LED = 1;
Lightvalve 257:c93d3eabff75 676 //// LED = 1;
Lightvalve 257:c93d3eabff75 677 // }
Lightvalve 257:c93d3eabff75 678 //// float alpha_update = 1.0f / (1.0f + FREQ_TMR2 / (2.0f * 3.14f * LVDT_f_cut)); // f_cutoff : 100Hz
Lightvalve 257:c93d3eabff75 679 //// LVDT_LPF = (1.0f - alpha_update) * LVDT_LPF + alpha_update * LVDT_new;
Lightvalve 257:c93d3eabff75 680 //// LVDT_sum = LVDT_sum + LVDT_LPF;
Lightvalve 257:c93d3eabff75 681 // LVDT_sum = LVDT_sum + LVDT_new;
Lightvalve 257:c93d3eabff75 682 //
Lightvalve 257:c93d3eabff75 683 //// force.UpdateSen((((float)ADC1->DR) - 2047.5f), FREQ_TMR2, 100.0f); // unit : N
Lightvalve 257:c93d3eabff75 684 //// this->sen_diff = (sen_new-this->sen)*Freq_update;
Lightvalve 257:c93d3eabff75 685 //// float alpha_update = 1.0f / (1.0f + Freq_update / (2.0f * 3.14f * f_cut)); // f_cutoff : 100Hz
Lightvalve 257:c93d3eabff75 686 //// this->sen = (1.0f - alpha_update) * this->sen + alpha_update * sen_new;
Lightvalve 257:c93d3eabff75 687 //// force.sen = LVDT_new;
Lightvalve 257:c93d3eabff75 688 // }
Lightvalve 257:c93d3eabff75 689 //
Lightvalve 257:c93d3eabff75 690 // else if (TMR2_timer == 46) { //46
Lightvalve 257:c93d3eabff75 691 //// if (TMR2_timer == 50) {
Lightvalve 257:c93d3eabff75 692 //// LED = 1;
Lightvalve 257:c93d3eabff75 693 //// ADC1->CR2 |= 0x40000000;
Lightvalve 257:c93d3eabff75 694 //// LVDT_new = ((float)ADC1->DR) - 2047.5f;
Lightvalve 257:c93d3eabff75 695 //// force.sen = LVDT_new;
Lightvalve 257:c93d3eabff75 696 // force.sen = LVDT_sum * 0.025f;
Lightvalve 257:c93d3eabff75 697 //// force.sen = LVDT_sum;
Lightvalve 257:c93d3eabff75 698 //
Lightvalve 257:c93d3eabff75 699 //// ADC1->CR2 |= 0x40000000;
Lightvalve 257:c93d3eabff75 700 //// LVDT_new = ((float)ADC1->DR) - 2047.5f;
Lightvalve 257:c93d3eabff75 701 //// force.sen = LVDT_new;
Lightvalve 257:c93d3eabff75 702 //// force.UpdateSen(LVDT_sum * 0.025f, 1000.0f, 10.0f);
Lightvalve 257:c93d3eabff75 703 //// LED = 0;
Lightvalve 257:c93d3eabff75 704 // }
Lightvalve 257:c93d3eabff75 705 //// LED = 0;
Lightvalve 257:c93d3eabff75 706 // toggle_old = toggle;
Lightvalve 257:c93d3eabff75 707 // TMR2_timer++;
Lightvalve 257:c93d3eabff75 708 // }
Lightvalve 257:c93d3eabff75 709 // TIM2->SR = 0x0; // reset the status register
Lightvalve 257:c93d3eabff75 710 //
Lightvalve 257:c93d3eabff75 711 //}