Skip to content

Instantly share code, notes, and snippets.

@kkmonster

kkmonster/TASK_01

Created November 17, 2014 18:15
Star
Embed
What would you like to do?
Learn more about clone URLs
Download ZIP
test gist
Raw
TASK_01
/**
******************************************************************************
* File Name : main.c
* Date : 03/11/2014 02:03:07
* Description : Main program body
******************************************************************************
*
* COPYRIGHT(c) 2014 STMicroelectronics
*
* Redistribution and use in source and binary forms, with or without modification,
* are permitted provided that the following conditions are met:
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
* 3. Neither the name of STMicroelectronics nor the names of its contributors
* may be used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*
******************************************************************************
*/
/* Includes ------------------------------------------------------------------*/
#include "stm32f4xx_hal.h"
/* USER CODE BEGIN Includes */
#define ARM_MATH_CM4
#include <math.h>
#include <arm_math.h>
#include <stdio.h>
#include <stdlib.h>
/* USER CODE END Includes */
/* Private variables ---------------------------------------------------------*/
ADC_HandleTypeDef hadc1;
ADC_HandleTypeDef hadc2;
ADC_HandleTypeDef hadc3;
SPI_HandleTypeDef hspi1;
SPI_HandleTypeDef hspi2;
SPI_HandleTypeDef hspi3;
TIM_HandleTypeDef htim1;
TIM_HandleTypeDef htim3;
TIM_HandleTypeDef htim4;
TIM_HandleTypeDef htim7;
UART_HandleTypeDef huart6;
DMA_HandleTypeDef hdma_usart6_rx;
/* USER CODE BEGIN PV */
/* USER CODE END PV */
/* Private function prototypes -----------------------------------------------*/
void SystemClock_Config(void);
static void MX_GPIO_Init(void);
static void MX_DMA_Init(void);
static void MX_ADC1_Init(void);
static void MX_ADC2_Init(void);
static void MX_ADC3_Init(void);
static void MX_SPI1_Init(void);
static void MX_SPI2_Init(void);
static void MX_SPI3_Init(void);
static void MX_TIM1_Init(void);
static void MX_TIM3_Init(void);
static void MX_TIM4_Init(void);
static void MX_TIM7_Init(void);
static void MX_USART6_UART_Init(void);
/* USER CODE BEGIN PFP */
/* USER CODE END PFP */
/* USER CODE BEGIN 0 */
/* define Queue parameter */
uint16_t Qsize = 0;
struct Node
{
float x;
float y;
float z;
float v;
struct Node* next;
};
typedef struct Node Node ;
Node *front;
Node *rear;
/* define constants parameter */
/* define Robot Parameters start */
#define R_Base 290.0f // radies of base
#define r_Eff 80.0f // radies of end effector
#define n_1 0.0f /180.0f*PI // angle of the motor 1 installed from x axis
#define n_2 120.0f /180.0f*PI // angle of the motor 2 installed from x axis
#define n_3 240.0f /180.0f*PI // angle of the motor 3 installed from x axis
#define lb_1 243.0f // the upper arms Lb 1
#define lb_2 243.0f // the upper arms Lb 2
#define lb_3 243.0f // the upper arms Lb 3
#define la_1 570.0f // the lowwer arms La 1
#define la_2 570.0f // the lowwer arms La 2
#define la_3 570.0f // the lowwer arms La 3
#define Vlimmit 10.0f // Velocity limmit mm/s
#define Alimmit 10.0f // Accelleration limmit mm/s
float n[3] = {n_1, n_2, n_3 } ;
float lb[3] = {lb_1, lb_2, lb_3 } ;
float la[3] = {la_1, la_2, la_3 } ;
float xxa,xxb,xxc;
/* define Robot Parameters start */
#define Sampling 800.0f // Hz
#define T 3.0f // Period of 30 degree oscillation
#define M_PI 3.142857143f
#define value_serial_prescale 4.0f // set: 50 hz ; value_serial_prescale = (200hz/serial comunication_rate)
#define limmit_output 500.0f
#define death_band 500.0f
#define static_culomn_A_cw 82.0f
#define static_culomn_A_ccw -72.0f
#define static_culomn_B_cw 75.0f
#define static_culomn_B_ccw -81.0f
#define static_culomn_C_cw 128.0f
#define static_culomn_C_ccw -109.0f
#define dynamic_culomn_A_cw 52.0f
#define dynamic_culomn_A_ccw -58.0f
#define dynamic_culomn_B_cw 63.0f
#define dynamic_culomn_B_ccw -68.0f
#define dynamic_culomn_C_cw 94.0f // approximate
#define dynamic_culomn_C_ccw -94.0f
/* Encoder initialzation */
TIM_Encoder_InitTypeDef hEncoder1 ;
TIM_Encoder_InitTypeDef hEncoder3 ;
TIM_Encoder_InitTypeDef hEncoder4 ;
/* Glabal Variable static */
volatile uint8_t mode = 0; //0 for line 1 for circle
volatile uint16_t step = 0; // step for feed = 200Hz / 10 Hz =20
volatile uint8_t count = 0; // step of time feed 10Hz
float buf_raw_ref_A=0, buf_raw_ref_B=0, buf_raw_ref_C=0 ;
uint16_t raw16_ref_A=0, raw16_ref_B=0, raw16_ref_C=0 ;
uint8_t buffer_uart[6];
// prescale for 50 hz serial conversation
float xx =100, yy =100, zz = -445 ;
float inv_test[3] = {0};
uint8_t serial_prescale = 0 ;
double ref0 = 0 ;
double time = 0 ;
/* Glabal Variable */
volatile int8_t Emer_ST = 0; // if Emer_ST = 1 enable emergency STOP system
volatile int8_t set_zero_A=1, set_zero_B=1, set_zero_C=1;
volatile int8_t state_set_zero_A=0, state_set_zero_B=0, state_set_zero_C=0;
volatile int32_t raw_dA=0, raw_dB=0, raw_dC=0;
volatile float raw_ref_A=-8.86527289359090, raw_ref_B=-61.3888733903427, raw_ref_C=-61.3888733903427 ;
/* test Variable */
int32_t buffer_B = 0 ,i=0, j=0 , avg=0, sum=0;
int8_t hold_serial = 0 ;
/* Values of ERROR Variable */
volatile static float error_A =0, error_B =0, error_C =0;
volatile static float ierror_A =0, ierror_B=0, ierror_C=0;
volatile static float derror_A =0, derror_B=0, derror_C=0;
/* Values of ERROR*Gain */
volatile static float aV_Kp_err =0, aV_Ki_err=0, aV_Kd_err=0;
volatile static float bV_Kp_err =0, bV_Ki_err=0, bV_Kd_err=0;
volatile static float cV_Kp_err =0, cV_Ki_err=0, cV_Kd_err=0;
/* Gain Of Controller */
volatile static float Gain_Kp =200 ; // Controller Gain (PID)
volatile static float Gain_Kd =2.9 ;
volatile static float Gain_Ki =0 ;
volatile static float state_dA=0, state_dB=0, state_dC=0; // states position of actuator
volatile static float state_vA=0, state_vB=0, state_vC=0; // states velocity of actuator
volatile static int16_t out_A = 0, out_B = 0, out_C = 0 ; // outputs(torque) to motors
volatile static float ref_A =0, ref_B=0, ref_C=0; // position control (DEG)
/* Mask for DAC */
const uint16_t mpc4822_ch_A=0x1000, mpc4822_ch_B=0x9000;
/* Function Prototype */
void Controller(void);
void Write_DAC(int16_t D2motorA,int16_t D2motorB,int16_t D2motorC ); // write output
void read_data_serial(void);
void sent_data_serial(volatile int32_t *angle);
void initial_encoder(void);
void encoder_reset(void);
void encoder_reset_A(void);
void encoder_reset_B(void);
void encoder_reset_C(void);
void encoder_read(void);
void encoder_ch_C_motor_A(void);
void encoder_ch_C_motor_B(void);
void encoder_ch_C_motor_C(void);
void sent_header_serial(void);
void sent_terminator_serial(void);
void update_trajectory(void);
void inverse_kinematics(float x, float y, float z, float thata[3]) ;
float *trajectory_planning_blendtime(float thata_past, float thata, float speed);
void add_Queue(float x, float y, float z, float v);
Node *get_Queue(void);
void remove_Queue(Node *tmp);
Node *newNode(float a, float b, float c, float d); // internal for initialize malloc
/* USER CODE END 0 */
int main(void)
{
/* USER CODE BEGIN 1 */
/* USER CODE END 1 */
/* MCU Configuration----------------------------------------------------------*/
/* Reset of all peripherals, Initializes the Flash interface and the Systick. */
HAL_Init();
/* Configure the system clock */
SystemClock_Config();
/* Initialize all configured peripherals */
MX_GPIO_Init();
MX_DMA_Init();
MX_ADC1_Init();
MX_ADC2_Init();
MX_ADC3_Init();
MX_SPI1_Init();
MX_SPI2_Init();
MX_SPI3_Init();
MX_TIM1_Init();
MX_TIM3_Init();
MX_TIM4_Init();
MX_TIM7_Init();
MX_USART6_UART_Init();
/* USER CODE BEGIN 2 */
initial_encoder(); // init encoder
for(uint8_t index = 0 ;index <50; index++)
{
Write_DAC(0, 0, 0); // set motor driver to zero
HAL_Delay(10);
}
/* USER CODE END 2 */
/* USER CODE BEGIN 3 */
/* Infinite loop */
HAL_GPIO_WritePin(GPIOD,GPIO_PIN_12, GPIO_PIN_SET); // green led for enable driver
HAL_Delay(5000);
HAL_GPIO_WritePin(GPIOD,GPIO_PIN_12, GPIO_PIN_RESET);
encoder_reset(); // set encoder to zero
/*set zero*/
set_zero_A=1;
set_zero_B=1;
set_zero_C=1;
HAL_TIM_Base_Start_IT(&htim7);
while (1)
{
}
/* USER CODE END 3 */
}
/** System Clock Configuration
*/
void SystemClock_Config(void)
{
RCC_OscInitTypeDef RCC_OscInitStruct;
RCC_ClkInitTypeDef RCC_ClkInitStruct;
__PWR_CLK_ENABLE();
__HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE1);
RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSE;
RCC_OscInitStruct.HSEState = RCC_HSE_ON;
RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE;
RCC_OscInitStruct.PLL.PLLM = 8;
RCC_OscInitStruct.PLL.PLLN = 336;
RCC_OscInitStruct.PLL.PLLP = RCC_PLLP_DIV2;
RCC_OscInitStruct.PLL.PLLQ = 4;
HAL_RCC_OscConfig(&RCC_OscInitStruct);
RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_SYSCLK|RCC_CLOCKTYPE_PCLK1
|RCC_CLOCKTYPE_PCLK2;
RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV4;
RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV2;
HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_5);
}
/* ADC1 init function */
void MX_ADC1_Init(void)
{
ADC_ChannelConfTypeDef sConfig;
/**Configure the global features of the ADC (Clock, Resolution, Data Alignment and number of conversion)
*/
hadc1.Instance = ADC1;
hadc1.Init.ClockPrescaler = ADC_CLOCKPRESCALER_PCLK_DIV2;
hadc1.Init.Resolution = ADC_RESOLUTION12b;
hadc1.Init.ScanConvMode = DISABLE;
hadc1.Init.ContinuousConvMode = DISABLE;
hadc1.Init.DiscontinuousConvMode = DISABLE;
hadc1.Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_NONE;
hadc1.Init.DataAlign = ADC_DATAALIGN_RIGHT;
hadc1.Init.NbrOfConversion = 1;
hadc1.Init.DMAContinuousRequests = DISABLE;
hadc1.Init.EOCSelection = EOC_SINGLE_CONV;
HAL_ADC_Init(&hadc1);
/**Configure for the selected ADC regular channel its corresponding rank in the sequencer and its sample time.
*/
sConfig.Channel = ADC_CHANNEL_10;
sConfig.Rank = 1;
sConfig.SamplingTime = ADC_SAMPLETIME_3CYCLES;
HAL_ADC_ConfigChannel(&hadc1, &sConfig);
}
/* ADC2 init function */
void MX_ADC2_Init(void)
{
ADC_ChannelConfTypeDef sConfig;
/**Configure the global features of the ADC (Clock, Resolution, Data Alignment and number of conversion)
*/
hadc2.Instance = ADC2;
hadc2.Init.ClockPrescaler = ADC_CLOCKPRESCALER_PCLK_DIV2;
hadc2.Init.Resolution = ADC_RESOLUTION12b;
hadc2.Init.ScanConvMode = DISABLE;
hadc2.Init.ContinuousConvMode = DISABLE;
hadc2.Init.DiscontinuousConvMode = DISABLE;
hadc2.Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_NONE;
hadc2.Init.DataAlign = ADC_DATAALIGN_RIGHT;
hadc2.Init.NbrOfConversion = 1;
hadc2.Init.DMAContinuousRequests = DISABLE;
hadc2.Init.EOCSelection = EOC_SINGLE_CONV;
HAL_ADC_Init(&hadc2);
/**Configure for the selected ADC regular channel its corresponding rank in the sequencer and its sample time.
*/
sConfig.Channel = ADC_CHANNEL_11;
sConfig.Rank = 1;
sConfig.SamplingTime = ADC_SAMPLETIME_3CYCLES;
HAL_ADC_ConfigChannel(&hadc2, &sConfig);
}
/* ADC3 init function */
void MX_ADC3_Init(void)
{
ADC_ChannelConfTypeDef sConfig;
/**Configure the global features of the ADC (Clock, Resolution, Data Alignment and number of conversion)
*/
hadc3.Instance = ADC3;
hadc3.Init.ClockPrescaler = ADC_CLOCKPRESCALER_PCLK_DIV2;
hadc3.Init.Resolution = ADC_RESOLUTION12b;
hadc3.Init.ScanConvMode = DISABLE;
hadc3.Init.ContinuousConvMode = DISABLE;
hadc3.Init.DiscontinuousConvMode = DISABLE;
hadc3.Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_NONE;
hadc3.Init.DataAlign = ADC_DATAALIGN_RIGHT;
hadc3.Init.NbrOfConversion = 1;
hadc3.Init.DMAContinuousRequests = DISABLE;
hadc3.Init.EOCSelection = EOC_SINGLE_CONV;
HAL_ADC_Init(&hadc3);
/**Configure for the selected ADC regular channel its corresponding rank in the sequencer and its sample time.
*/
sConfig.Channel = ADC_CHANNEL_12;
sConfig.Rank = 1;
sConfig.SamplingTime = ADC_SAMPLETIME_3CYCLES;
HAL_ADC_ConfigChannel(&hadc3, &sConfig);
}
/* SPI1 init function */
void MX_SPI1_Init(void)
{
hspi1.Instance = SPI1;
hspi1.Init.Mode = SPI_MODE_MASTER;
hspi1.Init.Direction = SPI_DIRECTION_1LINE;
hspi1.Init.DataSize = SPI_DATASIZE_16BIT;
hspi1.Init.CLKPolarity = SPI_POLARITY_LOW;
hspi1.Init.CLKPhase = SPI_PHASE_1EDGE;
hspi1.Init.NSS = SPI_NSS_HARD_OUTPUT;
hspi1.Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_8;
hspi1.Init.FirstBit = SPI_FIRSTBIT_MSB;
hspi1.Init.TIMode = SPI_TIMODE_DISABLED;
hspi1.Init.CRCCalculation = SPI_CRCCALCULATION_DISABLED;
HAL_SPI_Init(&hspi1);
}
/* SPI2 init function */
void MX_SPI2_Init(void)
{
hspi2.Instance = SPI2;
hspi2.Init.Mode = SPI_MODE_MASTER;
hspi2.Init.Direction = SPI_DIRECTION_1LINE;
hspi2.Init.DataSize = SPI_DATASIZE_16BIT;
hspi2.Init.CLKPolarity = SPI_POLARITY_LOW;
hspi2.Init.CLKPhase = SPI_PHASE_1EDGE;
hspi2.Init.NSS = SPI_NSS_SOFT;
hspi2.Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_4;
hspi2.Init.FirstBit = SPI_FIRSTBIT_MSB;
hspi2.Init.TIMode = SPI_TIMODE_DISABLED;
hspi2.Init.CRCCalculation = SPI_CRCCALCULATION_DISABLED;
HAL_SPI_Init(&hspi2);
}
/* SPI3 init function */
void MX_SPI3_Init(void)
{
hspi3.Instance = SPI3;
hspi3.Init.Mode = SPI_MODE_MASTER;
hspi3.Init.Direction = SPI_DIRECTION_1LINE;
hspi3.Init.DataSize = SPI_DATASIZE_16BIT;
hspi3.Init.CLKPolarity = SPI_POLARITY_LOW;
hspi3.Init.CLKPhase = SPI_PHASE_1EDGE;
hspi3.Init.NSS = SPI_NSS_SOFT;
hspi3.Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_4;
hspi3.Init.FirstBit = SPI_FIRSTBIT_MSB;
hspi3.Init.TIMode = SPI_TIMODE_DISABLED;
hspi3.Init.CRCCalculation = SPI_CRCCALCULATION_DISABLED;
HAL_SPI_Init(&hspi3);
}
/* TIM1 init function */
void MX_TIM1_Init(void)
{
TIM_SlaveConfigTypeDef sSlaveConfig;
TIM_MasterConfigTypeDef sMasterConfig;
TIM_IC_InitTypeDef sConfigIC;
htim1.Instance = TIM1;
htim1.Init.Prescaler = 0;
htim1.Init.CounterMode = TIM_COUNTERMODE_UP;
htim1.Init.Period = 65535;
htim1.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
htim1.Init.RepetitionCounter = 0;
HAL_TIM_Base_Init(&htim1);
HAL_TIM_IC_Init(&htim1);
sSlaveConfig.SlaveMode = TIM_SLAVEMODE_DISABLE;
sSlaveConfig.InputTrigger = TIM_TS_TI1F_ED;
sSlaveConfig.TriggerPolarity = TIM_INPUTCHANNELPOLARITY_RISING;
sSlaveConfig.TriggerFilter = 0;
HAL_TIM_SlaveConfigSynchronization(&htim1, &sSlaveConfig);
sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
HAL_TIMEx_MasterConfigSynchronization(&htim1, &sMasterConfig);
sConfigIC.ICPolarity = TIM_INPUTCHANNELPOLARITY_RISING;
sConfigIC.ICSelection = TIM_ICSELECTION_INDIRECTTI;
sConfigIC.ICPrescaler = TIM_ICPSC_DIV1;
sConfigIC.ICFilter = 0;
HAL_TIM_IC_ConfigChannel(&htim1, &sConfigIC, TIM_CHANNEL_1);
sConfigIC.ICSelection = TIM_ICSELECTION_DIRECTTI;
HAL_TIM_IC_ConfigChannel(&htim1, &sConfigIC, TIM_CHANNEL_2);
}
/* TIM3 init function */
void MX_TIM3_Init(void)
{
TIM_SlaveConfigTypeDef sSlaveConfig;
TIM_MasterConfigTypeDef sMasterConfig;
TIM_IC_InitTypeDef sConfigIC;
htim3.Instance = TIM3;
htim3.Init.Prescaler = 0;
htim3.Init.CounterMode = TIM_COUNTERMODE_UP;
htim3.Init.Period = 65535;
htim3.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
HAL_TIM_Base_Init(&htim3);
HAL_TIM_IC_Init(&htim3);
sSlaveConfig.SlaveMode = TIM_SLAVEMODE_DISABLE;
sSlaveConfig.InputTrigger = TIM_TS_TI1F_ED;
sSlaveConfig.TriggerPolarity = TIM_INPUTCHANNELPOLARITY_RISING;
sSlaveConfig.TriggerFilter = 0;
HAL_TIM_SlaveConfigSynchronization(&htim3, &sSlaveConfig);
sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
HAL_TIMEx_MasterConfigSynchronization(&htim3, &sMasterConfig);
sConfigIC.ICPolarity = TIM_INPUTCHANNELPOLARITY_RISING;
sConfigIC.ICSelection = TIM_ICSELECTION_INDIRECTTI;
sConfigIC.ICPrescaler = TIM_ICPSC_DIV1;
sConfigIC.ICFilter = 0;
HAL_TIM_IC_ConfigChannel(&htim3, &sConfigIC, TIM_CHANNEL_1);
sConfigIC.ICSelection = TIM_ICSELECTION_DIRECTTI;
HAL_TIM_IC_ConfigChannel(&htim3, &sConfigIC, TIM_CHANNEL_2);
}
/* TIM4 init function */
void MX_TIM4_Init(void)
{
TIM_SlaveConfigTypeDef sSlaveConfig;
TIM_MasterConfigTypeDef sMasterConfig;
TIM_IC_InitTypeDef sConfigIC;
htim4.Instance = TIM4;
htim4.Init.Prescaler = 0;
htim4.Init.CounterMode = TIM_COUNTERMODE_UP;
htim4.Init.Period = 65535;
htim4.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
HAL_TIM_Base_Init(&htim4);
HAL_TIM_IC_Init(&htim4);
sSlaveConfig.SlaveMode = TIM_SLAVEMODE_DISABLE;
sSlaveConfig.InputTrigger = TIM_TS_TI1F_ED;
sSlaveConfig.TriggerPolarity = TIM_INPUTCHANNELPOLARITY_RISING;
sSlaveConfig.TriggerFilter = 0;
HAL_TIM_SlaveConfigSynchronization(&htim4, &sSlaveConfig);
sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
HAL_TIMEx_MasterConfigSynchronization(&htim4, &sMasterConfig);
sConfigIC.ICPolarity = TIM_INPUTCHANNELPOLARITY_RISING;
sConfigIC.ICSelection = TIM_ICSELECTION_INDIRECTTI;
sConfigIC.ICPrescaler = TIM_ICPSC_DIV1;
sConfigIC.ICFilter = 0;
HAL_TIM_IC_ConfigChannel(&htim4, &sConfigIC, TIM_CHANNEL_1);
sConfigIC.ICSelection = TIM_ICSELECTION_DIRECTTI;
HAL_TIM_IC_ConfigChannel(&htim4, &sConfigIC, TIM_CHANNEL_2);
}
/* TIM7 init function */
void MX_TIM7_Init(void)
{
TIM_MasterConfigTypeDef sMasterConfig;
htim7.Instance = TIM7;
htim7.Init.Prescaler = 839;
htim7.Init.CounterMode = TIM_COUNTERMODE_UP;
htim7.Init.Period = 124;
HAL_TIM_Base_Init(&htim7);
sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
HAL_TIMEx_MasterConfigSynchronization(&htim7, &sMasterConfig);
}
/* USART6 init function */
void MX_USART6_UART_Init(void)
{
huart6.Instance = USART6;
huart6.Init.BaudRate = 115200;
huart6.Init.WordLength = UART_WORDLENGTH_8B;
huart6.Init.StopBits = UART_STOPBITS_1;
huart6.Init.Parity = UART_PARITY_NONE;
huart6.Init.Mode = UART_MODE_TX_RX;
huart6.Init.HwFlowCtl = UART_HWCONTROL_NONE;
huart6.Init.OverSampling = UART_OVERSAMPLING_8;
HAL_UART_Init(&huart6);
}
/**
* Enable DMA controller clock
*/
void MX_DMA_Init(void)
{
/* DMA controller clock enable */
__DMA2_CLK_ENABLE();
/* DMA interrupt init */
}
/** Configure pins as
* Analog
* Input
* Output
* EVENT_OUT
* EXTI
*/
void MX_GPIO_Init(void)
{
GPIO_InitTypeDef GPIO_InitStruct;
/* GPIO Ports Clock Enable */
__GPIOE_CLK_ENABLE();
__GPIOH_CLK_ENABLE();
__GPIOC_CLK_ENABLE();
__GPIOA_CLK_ENABLE();
__GPIOB_CLK_ENABLE();
__GPIOD_CLK_ENABLE();
/*Configure GPIO pins : PE2 PE4 PE6 PE7
PE9 PE11 */
GPIO_InitStruct.Pin = GPIO_PIN_2|GPIO_PIN_4|GPIO_PIN_6|GPIO_PIN_7
|GPIO_PIN_9|GPIO_PIN_11;
GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
GPIO_InitStruct.Pull = GPIO_NOPULL;
HAL_GPIO_Init(GPIOE, &GPIO_InitStruct);
/*Configure GPIO pins : PE3 PE5 PE1 */
GPIO_InitStruct.Pin = GPIO_PIN_3|GPIO_PIN_5|GPIO_PIN_1;
GPIO_InitStruct.Mode = GPIO_MODE_IT_RISING;
GPIO_InitStruct.Pull = GPIO_NOPULL;
HAL_GPIO_Init(GPIOE, &GPIO_InitStruct);
/*Configure GPIO pin : PA0 */
GPIO_InitStruct.Pin = GPIO_PIN_0;
GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
GPIO_InitStruct.Pull = GPIO_NOPULL;
HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
/*Configure GPIO pins : PA4 PA15 */
GPIO_InitStruct.Pin = GPIO_PIN_4|GPIO_PIN_15;
GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
GPIO_InitStruct.Pull = GPIO_NOPULL;
GPIO_InitStruct.Speed = GPIO_SPEED_FAST;
HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
/*Configure GPIO pin : PB12 */
GPIO_InitStruct.Pin = GPIO_PIN_12;
GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
GPIO_InitStruct.Pull = GPIO_NOPULL;
GPIO_InitStruct.Speed = GPIO_SPEED_FAST;
HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
/*Configure GPIO pins : PD12 PD13 PD14 PD15 */
GPIO_InitStruct.Pin = GPIO_PIN_12|GPIO_PIN_13|GPIO_PIN_14|GPIO_PIN_15;
GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
GPIO_InitStruct.Pull = GPIO_NOPULL;
GPIO_InitStruct.Speed = GPIO_SPEED_LOW;
HAL_GPIO_Init(GPIOD, &GPIO_InitStruct);
/* EXTI interrupt init*/
HAL_NVIC_SetPriority(EXTI1_IRQn, 1, 0);
HAL_NVIC_EnableIRQ(EXTI1_IRQn);
HAL_NVIC_SetPriority(EXTI3_IRQn, 1, 0);
HAL_NVIC_EnableIRQ(EXTI3_IRQn);
HAL_NVIC_SetPriority(EXTI9_5_IRQn, 1, 0);
HAL_NVIC_EnableIRQ(EXTI9_5_IRQn);
}
/* USER CODE BEGIN 4 */
void Controller(void)
{
/* blue led on for control working */
HAL_GPIO_WritePin(GPIOD,GPIO_PIN_15, GPIO_PIN_SET);
/* for find case of feedforward static or dynamic */
uint32_t buffer_raw_dA=raw_dA;
uint32_t buffer_raw_dB=raw_dB;
uint32_t buffer_raw_dC=raw_dC;
/* for find diff */
float buffer_error_A = error_A;
float buffer_error_B = error_B;
float buffer_error_C = error_C;
encoder_read();
/* USER CODE Controller */
/* Set Zero All motor */
if(set_zero_A) ref_A += 0.02f;
if(set_zero_B) ref_B += 0.02f;
if(set_zero_C) ref_C += 0.02f;
int8_t buf_set_zero_A=state_set_zero_A;
int8_t buf_set_zero_B=state_set_zero_B;
int8_t buf_set_zero_C=state_set_zero_C;
state_set_zero_A=HAL_GPIO_ReadPin(GPIOE,GPIO_PIN_2) ;
state_set_zero_B=HAL_GPIO_ReadPin(GPIOE,GPIO_PIN_4) ;
state_set_zero_C=HAL_GPIO_ReadPin(GPIOE,GPIO_PIN_6) ;
if(state_set_zero_A==1 && !buf_set_zero_A)encoder_reset_A();
if(state_set_zero_B==1 && !buf_set_zero_B)encoder_reset_B();
if(state_set_zero_C==1 && !buf_set_zero_C)encoder_reset_C();
/* PID Controller */
error_A = ref_A - state_dA ;
ierror_A += error_A / Sampling ;
derror_A = (error_A - buffer_error_A)*Sampling ;
aV_Kp_err = error_A *Gain_Kp;
aV_Ki_err = ierror_A *Gain_Ki;
aV_Kd_err = derror_A *Gain_Kd;
out_A = aV_Kp_err+aV_Ki_err+aV_Kd_err;
error_B = ref_B - state_dB ;
ierror_B += error_B/Sampling ;
derror_B = (error_B - buffer_error_B)*Sampling ;
bV_Kp_err = error_B *Gain_Kp;
bV_Ki_err = ierror_B *Gain_Ki;
bV_Kd_err = derror_B *Gain_Kd;
out_B = bV_Kp_err+bV_Ki_err+bV_Kd_err;
error_C = ref_C - state_dC ;
ierror_C += error_C/Sampling ;
derror_C = (error_C - buffer_error_C)*Sampling ;
cV_Kp_err = error_C *Gain_Kp;
cV_Ki_err = ierror_C *Gain_Ki;
cV_Kd_err = derror_C *Gain_Kd;
out_C = cV_Kp_err+cV_Ki_err+cV_Kd_err;
/* USER CODE limmit output */
if(out_A > limmit_output) out_A = limmit_output ;
if(out_A <-limmit_output) out_A = -limmit_output ;
if(out_B > limmit_output) out_B = limmit_output ;
if(out_B <-limmit_output) out_B = -limmit_output ;
if(out_C > limmit_output) out_C = limmit_output ;
if(out_C <-limmit_output) out_C = -limmit_output ;
/* USER CODE culomn friction Feed forward static && dynamic */
if(raw_dA == buffer_raw_dA){
if(out_A > death_band) out_A += static_culomn_A_cw ;
if(out_A <-death_band) out_A += static_culomn_A_ccw ;
}else{
if(out_A > death_band) out_A += dynamic_culomn_A_cw ;
if(out_A <-death_band) out_A += dynamic_culomn_A_ccw ;
}
if(raw_dB == buffer_raw_dB){
if(out_B > death_band) out_B += static_culomn_B_cw ;
if(out_B <-death_band) out_B += static_culomn_B_ccw ;
}else{
if(out_B > death_band) out_B += dynamic_culomn_B_cw ;
if(out_B <-death_band) out_B += dynamic_culomn_B_ccw ;
}
if(raw_dC == buffer_raw_dC){
if(out_C > death_band) out_C += static_culomn_C_cw ;
if(out_C <-death_band) out_C += static_culomn_C_ccw ;
}else{
if(out_C > death_band) out_C += dynamic_culomn_C_cw ;
if(out_C <-death_band) out_C += dynamic_culomn_C_ccw ;
}
/* disable motor A, C */
out_A = 0;
out_C = 0;
/* output to driver */
Write_DAC(out_A, out_B, out_C);
inverse_kinematics(xx,yy,zz,inv_test);
xxa = inv_test[0];
xxb = inv_test[1];
xxc = inv_test[2];
update_trajectory();
HAL_GPIO_WritePin(GPIOD,GPIO_PIN_15, GPIO_PIN_RESET); // blue led off for control working
}
void Write_DAC(int16_t D2motorA,int16_t D2motorB,int16_t D2motorC )
{
D2motorA += 2049;
D2motorA &= 0x0fff;
D2motorB += 2049;
D2motorB &= 0x0fff;
D2motorC += 2049;
D2motorC &= 0x0fff;
uint16_t ch_A_D2motorA, ch_B_D2motorA, ch_A_D2motorB, ch_B_D2motorB, ch_A_D2motorC, ch_B_D2motorC;
ch_A_D2motorA = D2motorA ;
ch_A_D2motorA |= mpc4822_ch_A ;
ch_B_D2motorA = ((4094)-D2motorA) ;
ch_B_D2motorA |= mpc4822_ch_B ;
ch_A_D2motorB = D2motorB ;
ch_A_D2motorB |= mpc4822_ch_A ;
ch_B_D2motorB = ((4094)-D2motorB) ;
ch_B_D2motorB |= mpc4822_ch_B ;
ch_A_D2motorC = D2motorC ;
ch_A_D2motorC |= mpc4822_ch_A ;
ch_B_D2motorC = ((4094)-D2motorC) ;
ch_B_D2motorC |= mpc4822_ch_B ;
HAL_GPIO_WritePin(GPIOA,GPIO_PIN_15, GPIO_PIN_RESET);
HAL_SPI_Transmit(&hspi3, (uint8_t*)&ch_A_D2motorA, 1,1); //write data to driver A ch_A
HAL_GPIO_WritePin(GPIOA,GPIO_PIN_15, GPIO_PIN_SET);
HAL_GPIO_WritePin(GPIOA,GPIO_PIN_15, GPIO_PIN_RESET);
HAL_SPI_Transmit(&hspi3, (uint8_t*)&ch_B_D2motorA, 1,1); //write data to driver A ch_B
HAL_GPIO_WritePin(GPIOA,GPIO_PIN_15, GPIO_PIN_SET);
HAL_GPIO_WritePin(GPIOA,GPIO_PIN_4, GPIO_PIN_RESET);
HAL_SPI_Transmit(&hspi1, (uint8_t*)&ch_A_D2motorB, 1,1); //write data to driver B ch_A
HAL_GPIO_WritePin(GPIOA,GPIO_PIN_4, GPIO_PIN_SET);
HAL_GPIO_WritePin(GPIOA,GPIO_PIN_4, GPIO_PIN_RESET);
HAL_SPI_Transmit(&hspi1, (uint8_t*)&ch_B_D2motorB, 1,1); //write data to driver B ch_B
HAL_GPIO_WritePin(GPIOA,GPIO_PIN_4, GPIO_PIN_SET);
HAL_GPIO_WritePin(GPIOB,GPIO_PIN_12, GPIO_PIN_RESET);
HAL_SPI_Transmit(&hspi2, (uint8_t*)&ch_A_D2motorC, 1,1); //write data to driver C ch_A
HAL_GPIO_WritePin(GPIOB,GPIO_PIN_12, GPIO_PIN_SET);
HAL_GPIO_WritePin(GPIOB,GPIO_PIN_12, GPIO_PIN_RESET);
HAL_SPI_Transmit(&hspi2, (uint8_t*)&ch_B_D2motorC, 1,1); //write data to driver C ch_B
HAL_GPIO_WritePin(GPIOB,GPIO_PIN_12, GPIO_PIN_SET);
}
void initial_encoder(void)
{
hEncoder1.EncoderMode = TIM_ENCODERMODE_TI1 ;
hEncoder1.IC1Polarity = TIM_INPUTCHANNELPOLARITY_FALLING;
hEncoder1.IC2Polarity = TIM_INPUTCHANNELPOLARITY_RISING;
HAL_TIM_Encoder_Init(&htim1,&hEncoder1);
HAL_TIM_Encoder_Start_IT(&htim1,TIM_CHANNEL_1 | TIM_CHANNEL_2);
hEncoder3.EncoderMode = TIM_ENCODERMODE_TI1 ;
hEncoder3.IC1Polarity = TIM_INPUTCHANNELPOLARITY_FALLING;
hEncoder3.IC2Polarity = TIM_INPUTCHANNELPOLARITY_RISING;
HAL_TIM_Encoder_Init(&htim3,&hEncoder3);
HAL_TIM_Encoder_Start_IT(&htim3,TIM_CHANNEL_1 | TIM_CHANNEL_2);
hEncoder4.EncoderMode = TIM_ENCODERMODE_TI1 ;
hEncoder4.IC1Polarity = TIM_INPUTCHANNELPOLARITY_FALLING;
hEncoder4.IC2Polarity = TIM_INPUTCHANNELPOLARITY_RISING;
HAL_TIM_Encoder_Init(&htim4,&hEncoder4);
HAL_TIM_Encoder_Start_IT(&htim4,TIM_CHANNEL_1 | TIM_CHANNEL_2);
}
void encoder_reset(void)
{
encoder_reset_A();
encoder_reset_B();
encoder_reset_C();
}
void encoder_reset_A(void)
{
TIM4->CNT = 0x3fff ; raw_dA=4167; state_vA=0; out_A = 0; set_zero_A=0; ref_A = 4167* 0.0036 ;
error_A = 0;
ierror_A =0;
derror_A =0;
}
void encoder_reset_B(void)
{
TIM3->CNT = 0x3fff ; raw_dB=4167; state_vB=0; out_B = 0; set_zero_B=0; ref_B = 4167* 0.0036 ;
error_B = 0;
ierror_B =0;
derror_B =0;
}
void encoder_reset_C(void)
{
TIM1->CNT = 0x3fff ; raw_dC=4167; state_vC=0; out_C = 0; set_zero_C=0; ref_C = 4167* 0.0036 ;
error_C = 0;
ierror_C =0;
derror_C =0;
}
void encoder_read(void)
{
int16_t buf_state_A, buf_state_B, buf_state_C ;
buf_state_A = TIM4->CNT - 0x3fff ;
TIM4->CNT = 0x3fff ;
buf_state_B = TIM3->CNT - 0x3fff ;
TIM3->CNT = 0x3fff ;
buf_state_C = TIM1->CNT - 0x3fff ;
TIM1->CNT = 0x3fff ;
raw_dA -= buf_state_A;
raw_dB -= buf_state_B;
raw_dC -= buf_state_C;
state_dA = raw_dA * 0.0036 ;
state_dB = raw_dB * 0.0036 ;
state_dC = raw_dC * 0.0036 ;
}
void sent_data_serial(volatile int32_t *angle )
{
uint8_t Buffer_angle_motor_B[4] ;
// uint8_t Buffer_torque_motor_B[2];
/* Sent header */
sent_header_serial();
/* Sent data */
//
// uint16 Torque Motor B
// Buffer_torque_motor_B[0] = *torque ;
// Buffer_torque_motor_B[1] = *torque>>8 ;
// HAL_UART_Transmit(&huart6, (uint8_t*)Buffer_torque_motor_B, 2 ,1);
// int32 Angle Motor B
Buffer_angle_motor_B[0] = *angle ;
Buffer_angle_motor_B[1] = *angle >>8 ;
Buffer_angle_motor_B[2] = *angle >>16 ;
Buffer_angle_motor_B[3] = *angle >>24 ;
HAL_UART_Transmit(&huart6, (uint8_t*)Buffer_angle_motor_B, 4 ,1);
/* Sent terminator */
sent_terminator_serial();
}
void sent_header_serial(void)
{
const uint8_t header[1] = {0x7E} ;
const uint8_t enable[1] = {0x01} ;
const uint8_t terminal[1] = {0x70} ;
HAL_UART_Transmit(&huart6, (uint8_t*)header, 1 ,1);
HAL_UART_Transmit(&huart6, (uint8_t*)enable, 1 ,1);
HAL_UART_Transmit(&huart6, (uint8_t*)terminal, 1 ,1);
}
void sent_terminator_serial(void)
{
const uint8_t terminator[2] = {0x03, 0x03} ;
HAL_UART_Transmit(&huart6, (uint8_t*)terminator, 2 ,1);
}
void read_data_serial(void)
{
HAL_UART_Receive_IT(&huart6,buffer_uart,6);
}
void update_trajectory(void)
{
/* Update mode trajectory */
if(!set_zero_A && !set_zero_B && !set_zero_C && !mode) mode = 1 ;
/* mode = 2 feed cir trajectory */
if(mode == 1)
{
if (!HAL_GPIO_ReadPin(GPIOA,GPIO_PIN_0) & !hold_serial) // waitting state
{
HAL_GPIO_WritePin(GPIOD,GPIO_PIN_12, GPIO_PIN_SET);
HAL_GPIO_WritePin(GPIOD,GPIO_PIN_12, GPIO_PIN_RESET);
ref0 = ref_B ;
}
else
{
hold_serial = 1 ; // cancel waitting state
static Node *tmp;
static double time;
static double end_time;
static double magnitude;
static float tmp_x;
static float tmp_y;
static float tmp_z;
static float tmp_v;
static float tmp_old_x;
static float tmp_old_y;
static float tmp_old_z;
if(time >= end_time && Qsize > 0) // get next point, solve inv-kinematics & solve trajectory p
{
time = 0;
/* mem old point */
tmp_old_x = tmp->x;
tmp_old_y = tmp->y;
tmp_old_z = tmp->z;
/* Get Queue */
tmp = get_Queue();
tmp_x = tmp->x;
tmp_y = tmp->y;
tmp_z = tmp->z;
tmp_v = tmp->v;
/* delete Queue's mem */
remove_Queue(tmp);
/* magnitude */
magnitude = sqrtf(tmp_x*tmp_x + tmp_y*tmp_y + tmp_z*tmp_z);
/* unit_vector norm */
tmp_x /= magnitude;
tmp_y /= magnitude;
tmp_z /= magnitude;
= trajectory_planning_blendtime(float thata_past, float thata, float speed) ; // return
}
/* update referent point in task space from trajectory planing */
/* inverse kinematics from trajectory planing in task space to joint space */
inverse_kinematics(float x, float y, float z, float thata[3])
}
}
}
void update_gain(void)
{
HAL_ADC_Start_IT(&hadc1);
HAL_ADC_Start_IT(&hadc2);
HAL_ADC_Start_IT(&hadc3);
}
void HAL_ADC_ConvCpltCallback(ADC_HandleTypeDef* hadc)
{
if(hadc->Instance == ADC1) Gain_Kp = HAL_ADC_GetValue(&hadc1) / 10;
if(hadc->Instance == ADC2) Gain_Ki = HAL_ADC_GetValue(&hadc2) / 100;
if(hadc->Instance == ADC3) Gain_Kp = HAL_ADC_GetValue(&hadc3) / 100;
}
void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart)
{
// HAL_GPIO_WritePin(GPIOD,GPIO_PIN_12, GPIO_PIN_SET);
HAL_GPIO_TogglePin(GPIOD,GPIO_PIN_12);
buf_raw_ref_A = raw_ref_A ;
buf_raw_ref_B = raw_ref_B ;
buf_raw_ref_C = raw_ref_C ;
raw16_ref_A= ((((uint16_t)buffer_uart[1])<<8) | ((uint16_t)buffer_uart[0])) ;
raw16_ref_B= ((((uint16_t)buffer_uart[3])<<8) | ((uint16_t)buffer_uart[2])) ;
raw16_ref_C= ((((uint16_t)buffer_uart[5])<<8) | ((uint16_t)buffer_uart[4])) ;
raw_ref_A= raw16_ref_A ;
raw_ref_B= raw16_ref_B ;
raw_ref_C= raw16_ref_C ;
raw_ref_A= raw_ref_A * (-360/65536);
raw_ref_B= raw_ref_B * (-360/65536);
raw_ref_C= raw_ref_C * (-360/65536);
/* max speed motor 300*+ rpm
gear ratio 50/1 motor speed become 60 rpm = 1 rps = 360 Deg
control loop 200Hz 360/200 = 1.8 deg / sampling*/
/*
if ((raw_ref_A - buf_raw_ref_A) > 1.8 ) raw_ref_A += 1.8;
if ((raw_ref_A - buf_raw_ref_A) <-1.8 ) raw_ref_A -= 1.8;
if ((raw_ref_B - buf_raw_ref_B) > 1.8 ) raw_ref_B += 1.8;
if ((raw_ref_B - buf_raw_ref_B) <-1.8 ) raw_ref_B -= 1.8;
if ((raw_ref_C - buf_raw_ref_C) > 1.8 ) raw_ref_C += 1.8;
if ((raw_ref_C - buf_raw_ref_C) <-1.8 ) raw_ref_C -= 1.8;
*/
// HAL_GPIO_WritePin(GPIOD,GPIO_PIN_12, GPIO_PIN_RESET);
}
void inverse_kinematics(float x, float y, float z, float thata[3])
{
float ax, ay, az ;
float ex, ey, ez ;
float r11,r12,r13,r21,r22,r23,r31,r32,r33 ;
float a1, jx, jz ;
for ( uint8_t i = 0 ; i < 3 ; i++ )
{
//Position of actuator
ax = R_Base*cos(n[i]) ;
ay = R_Base*cos(n[i]) ;
az = 0 ;
//Position of End-effector
ex = x + r_Eff*cos(n[i]) ;
ey = y + r_Eff*sin(n[i]) ;
ez = z ;
/* solve */
// rotation matrix
r11= cos(n[i]);
r12= sin(n[i]);
r13=0;
r21=-sin(n[i]);
r22= cos(n[i]);
r23=0;
r31=0;
r32=0;
r33=1;
// rotate position of base and eff
ax = r11*ax + r12*ax + r13*ax ;
ay = r21*ay + r22*ay + r23*ay ;
az = 0 ;
ex = r11*ex + r12*ex + r13*ex ;
ey = r21*ey + r22*ey + r23*ey ;
ez = r31*ez + r32*ez + r33*ez ;
ex = ex-ax ;
ey = ey-ay ;
ez = ez-az ;
a1 = atan2(ez,ex) ;
r11=-cos(-a1);
r12= 0;
r13=-sin(-a1);
r21= 0;
r22= 1;
r23= 0;
r31= sin(-a1);
r32= 0;
r33= cos(-a1);
ex = r11*ex + r12*ex + r13*ex ;
ey = r21*ey + r22*ey + r23*ey ;
ez = r31*ez + r32*ez + r33*ez ;
jx = (lb[i]*lb[i] - (la[i]*la[i] - ex*ex) + ex*ex)/(ex*ex);
jz = sqrt(lb[i]*lb[i] - jx*jx);
jx = r11*jx + r31*jz ;
jz = r13*jx + r33*jz ;
thata[i]= atan2(jz,jx)*180/PI ;
}
}
float *trajectory_planning_blendtime(float thata_past, float thata, float speed)
{
/* this function use to create trajectory from user control command */
// float t ;
return 0;
}
void add_Queue(float x, float y, float z, float v)
{
Qsize ++;
Node *tmp = rear ;
Node *ptr = newNode(x, y, z, v);
if (Qsize == 1) {
front = ptr;
rear = ptr;
} else {
rear = ptr;
tmp->next = ptr;
}
}
Node *get_Queue()
{
Node *tmp = front;
if (Qsize == 1) {
front = 0;
} else {
front = tmp->next ;
}
Qsize--;
return tmp;
}
void remove_Queue(Node *tmp)
{
free(tmp);
}
Node* newNode(float a, float b, float c, float d)
{
Node* ptr = malloc(1*sizeof(Node));
ptr->x = a;
ptr->y = b;
ptr->z = c;
ptr->v = d;
ptr->next = 0;
return ptr;
}
/* USER CODE END 4 */
#ifdef USE_FULL_ASSERT
/**
* @brief Reports the name of the source file and the source line number
* where the assert_param error has occurred.
* @param file: pointer to the source file name
* @param line: assert_param error line source number
* @retval None
*/
void assert_failed(uint8_t* file, uint32_t line)
{
/* USER CODE BEGIN 6 */
/* User can add his own implementation to report the file name and line number,
ex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */
/* USER CODE END 6 */
}
#endif
/**
* @}
*/
/**
* @}
*/
/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment