ozgur erbulan / Mbed 2 deprecated ROS-kinetic-cli

test

Dependencies:   mbed-rtos mbed ros_kinetic

main.cpp@0:9646fc4e3fae, 2017-05-19 (annotated)

Committer:
randalthor
Date:
Fri May 19 09:12:45 2017 +0000
Revision:
0:9646fc4e3fae
test

Who changed what in which revision?

UserRevisionLine numberNew contents of line
randalthor 0:9646fc4e3fae 1 #include "mbed.h"
randalthor 0:9646fc4e3fae 2 #include "cmsis_os.h"
randalthor 0:9646fc4e3fae 3 #include "main.h"
randalthor 0:9646fc4e3fae 4 #include "stm32f7xx_hal.h"
randalthor 0:9646fc4e3fae 5 #include <ros.h>
randalthor 0:9646fc4e3fae 6 #include <geometry_msgs/Twist.h>
randalthor 0:9646fc4e3fae 7 #include <geometry_msgs/Vector3.h>
randalthor 0:9646fc4e3fae 8 #include <openlab/mobileRobot.h>
randalthor 0:9646fc4e3fae 9 #include <std_msgs/String.h>
randalthor 0:9646fc4e3fae 10 #include <std_msgs/Float32.h>
randalthor 0:9646fc4e3fae 11 #include <std_msgs/Bool.h>
randalthor 0:9646fc4e3fae 12
randalthor 0:9646fc4e3fae 13 #define pi 3.1416 //for angular velocity calculation
randalthor 0:9646fc4e3fae 14 #define length 0.3 //distance between 2 wheel - meter
randalthor 0:9646fc4e3fae 15 #define radius 0.036 //radius of wheel with load - meter
randalthor 0:9646fc4e3fae 16 #define timeConst 0.00926 //time constant
randalthor 0:9646fc4e3fae 17
randalthor 0:9646fc4e3fae 18 /*Global variables*/
randalthor 0:9646fc4e3fae 19 int8_t ADC_buffer[2];
randalthor 0:9646fc4e3fae 20 int32_t encoder1_1 = 0;
randalthor 0:9646fc4e3fae 21 int32_t encoder1=0;
randalthor 0:9646fc4e3fae 22 int32_t encoder2_1 = 0;
randalthor 0:9646fc4e3fae 23 int32_t encoder2=0;
randalthor 0:9646fc4e3fae 24 float pwmLeft=0;
randalthor 0:9646fc4e3fae 25 float pwmRight=0;
randalthor 0:9646fc4e3fae 26 float dc_current1=0;
randalthor 0:9646fc4e3fae 27 float dc_current2=0;
randalthor 0:9646fc4e3fae 28 double real_velocity_linx=0;
randalthor 0:9646fc4e3fae 29 double real_velocity_angz=0;
randalthor 0:9646fc4e3fae 30 double cmd_velocity_linx=0;
randalthor 0:9646fc4e3fae 31 double cmd_velocity_angz=0;
randalthor 0:9646fc4e3fae 32
randalthor 0:9646fc4e3fae 33 float kp_left = 39;
randalthor 0:9646fc4e3fae 34 float kd_left = 2;
randalthor 0:9646fc4e3fae 35 float ki_left = 10;
randalthor 0:9646fc4e3fae 36 float kp_right = 39;
randalthor 0:9646fc4e3fae 37 float kd_right = 2;
randalthor 0:9646fc4e3fae 38 float ki_right = 10;
randalthor 0:9646fc4e3fae 39 double errDirVel_1 = 0;
randalthor 0:9646fc4e3fae 40 double errAngVel_1 = 0;
randalthor 0:9646fc4e3fae 41 double errorLeft = 0;
randalthor 0:9646fc4e3fae 42 double errorRight = 0;
randalthor 0:9646fc4e3fae 43 double errorLeft_1 = 0;
randalthor 0:9646fc4e3fae 44 double errorRight_1 = 0;
randalthor 0:9646fc4e3fae 45 double linearLeft = 0;
randalthor 0:9646fc4e3fae 46 double linearRight = 0;
randalthor 0:9646fc4e3fae 47 double driveLeft = 0;
randalthor 0:9646fc4e3fae 48 double driveRight = 0;
randalthor 0:9646fc4e3fae 49 double iLeft = 0;
randalthor 0:9646fc4e3fae 50 double iRight = 0;
randalthor 0:9646fc4e3fae 51
randalthor 0:9646fc4e3fae 52 float i_max = 1000;
randalthor 0:9646fc4e3fae 53 float dLeft = 0;
randalthor 0:9646fc4e3fae 54 float dRight = 0;
randalthor 0:9646fc4e3fae 55
randalthor 0:9646fc4e3fae 56 int diff1=0;
randalthor 0:9646fc4e3fae 57 int diff2=0;
randalthor 0:9646fc4e3fae 58 float current1;
randalthor 0:9646fc4e3fae 59 float current2;
randalthor 0:9646fc4e3fae 60
randalthor 0:9646fc4e3fae 61
randalthor 0:9646fc4e3fae 62
randalthor 0:9646fc4e3fae 63 ros::NodeHandle nh;
randalthor 0:9646fc4e3fae 64 openlab::mobileRobot str_msg;
randalthor 0:9646fc4e3fae 65 ros::Publisher RobotFeedback("RobotFeedback", &str_msg);
randalthor 0:9646fc4e3fae 66
randalthor 0:9646fc4e3fae 67 void callback_vel( const geometry_msgs::Twist& vel){
randalthor 0:9646fc4e3fae 68
randalthor 0:9646fc4e3fae 69 cmd_velocity_linx = vel.linear.x;
randalthor 0:9646fc4e3fae 70 cmd_velocity_angz = vel.angular.z;
randalthor 0:9646fc4e3fae 71
randalthor 0:9646fc4e3fae 72
randalthor 0:9646fc4e3fae 73 nh.spinOnce();
randalthor 0:9646fc4e3fae 74 osDelay(10);
randalthor 0:9646fc4e3fae 75 }
randalthor 0:9646fc4e3fae 76
randalthor 0:9646fc4e3fae 77 ros::Subscriber<geometry_msgs::Twist> sub("cmd_vel", callback_vel );
randalthor 0:9646fc4e3fae 78
randalthor 0:9646fc4e3fae 79 /*loop control*/
randalthor 0:9646fc4e3fae 80 //DigitalOut led1(LED1);
randalthor 0:9646fc4e3fae 81 DigitalOut led2(LED2);
randalthor 0:9646fc4e3fae 82 DigitalOut led3(LED3);
randalthor 0:9646fc4e3fae 83 /* USER CODE BEGIN Includes */
randalthor 0:9646fc4e3fae 84
randalthor 0:9646fc4e3fae 85 /* USER CODE END Includes */
randalthor 0:9646fc4e3fae 86
randalthor 0:9646fc4e3fae 87 /* Private variables ---------------------------------------------------------*/
randalthor 0:9646fc4e3fae 88 ADC_HandleTypeDef hadc2;
randalthor 0:9646fc4e3fae 89 DMA_HandleTypeDef hdma_adc2;
randalthor 0:9646fc4e3fae 90
randalthor 0:9646fc4e3fae 91 TIM_HandleTypeDef htim1;
randalthor 0:9646fc4e3fae 92 TIM_HandleTypeDef htim4;
randalthor 0:9646fc4e3fae 93 TIM_HandleTypeDef htim9;
randalthor 0:9646fc4e3fae 94
randalthor 0:9646fc4e3fae 95
randalthor 0:9646fc4e3fae 96 /* Private function prototypes -----------------------------------------------*/
randalthor 0:9646fc4e3fae 97
randalthor 0:9646fc4e3fae 98 void Error_Handler(void);
randalthor 0:9646fc4e3fae 99 static void MX_GPIO_Init(void);
randalthor 0:9646fc4e3fae 100 static void MX_DMA_Init(void);
randalthor 0:9646fc4e3fae 101 static void MX_ADC2_Init(void);
randalthor 0:9646fc4e3fae 102 static void MX_TIM1_Init(void);
randalthor 0:9646fc4e3fae 103 static void MX_TIM4_Init(void);
randalthor 0:9646fc4e3fae 104 static void MX_TIM9_Init(void);
randalthor 0:9646fc4e3fae 105
randalthor 0:9646fc4e3fae 106 void HAL_TIM_MspPostInit(TIM_HandleTypeDef *htim);
randalthor 0:9646fc4e3fae 107
randalthor 0:9646fc4e3fae 108 void feedback_thread(void const *args)
randalthor 0:9646fc4e3fae 109 {
randalthor 0:9646fc4e3fae 110 while (true) {
randalthor 0:9646fc4e3fae 111
randalthor 0:9646fc4e3fae 112
randalthor 0:9646fc4e3fae 113 str_msg.pwm_1 = pwmLeft;
randalthor 0:9646fc4e3fae 114 str_msg.pwm_2 = pwmRight;
randalthor 0:9646fc4e3fae 115 str_msg.real_velocity_linx = real_velocity_linx;
randalthor 0:9646fc4e3fae 116 str_msg.real_velocity_angz = real_velocity_angz;
randalthor 0:9646fc4e3fae 117 str_msg.cmd_velocity_linx = cmd_velocity_linx;
randalthor 0:9646fc4e3fae 118 str_msg.cmd_velocity_angz = cmd_velocity_angz;
randalthor 0:9646fc4e3fae 119 str_msg.dc_current_1 = current1;
randalthor 0:9646fc4e3fae 120 str_msg.dc_current_2 = current2;
randalthor 0:9646fc4e3fae 121 str_msg.encoder_1 = encoder1;
randalthor 0:9646fc4e3fae 122 str_msg.encoder_2 = encoder2;
randalthor 0:9646fc4e3fae 123 RobotFeedback.publish( &str_msg );
randalthor 0:9646fc4e3fae 124 nh.spinOnce();
randalthor 0:9646fc4e3fae 125 osDelay(10);
randalthor 0:9646fc4e3fae 126
randalthor 0:9646fc4e3fae 127 }
randalthor 0:9646fc4e3fae 128 }
randalthor 0:9646fc4e3fae 129
randalthor 0:9646fc4e3fae 130 void velocity_control(void const *args)
randalthor 0:9646fc4e3fae 131 {
randalthor 0:9646fc4e3fae 132
randalthor 0:9646fc4e3fae 133 while (true) {
randalthor 0:9646fc4e3fae 134
randalthor 0:9646fc4e3fae 135 current1 = ADC_buffer[0];
randalthor 0:9646fc4e3fae 136 current2 = ADC_buffer[1];
randalthor 0:9646fc4e3fae 137 encoder1 = TIM1->CNT;
randalthor 0:9646fc4e3fae 138 encoder2 = TIM4->CNT;
randalthor 0:9646fc4e3fae 139
randalthor 0:9646fc4e3fae 140
randalthor 0:9646fc4e3fae 141 diff1=encoder1 - encoder1_1;
randalthor 0:9646fc4e3fae 142 diff2=encoder2 - encoder2_1;
randalthor 0:9646fc4e3fae 143
randalthor 0:9646fc4e3fae 144 if(diff1 < - 30000) diff1 = diff1 + 65536;
randalthor 0:9646fc4e3fae 145 if(diff1 > 30000) diff1 = diff1 - 65536;
randalthor 0:9646fc4e3fae 146 if(diff2 < - 30000) diff2 = diff2 + 65536;
randalthor 0:9646fc4e3fae 147 if(diff2> 30000) diff2 = diff2 - 65536;
randalthor 0:9646fc4e3fae 148
randalthor 0:9646fc4e3fae 149 linearLeft = diff1 * 0.000748*200*radius;
randalthor 0:9646fc4e3fae 150 linearRight = diff2 * 0.000748*200*radius;
randalthor 0:9646fc4e3fae 151
randalthor 0:9646fc4e3fae 152 real_velocity_linx = (linearLeft + linearRight) * 0.5;
randalthor 0:9646fc4e3fae 153 real_velocity_angz = (linearRight - linearLeft) * 5.8824;
randalthor 0:9646fc4e3fae 154
randalthor 0:9646fc4e3fae 155 encoder1_1 = encoder1;
randalthor 0:9646fc4e3fae 156 encoder2_1 = encoder2;
randalthor 0:9646fc4e3fae 157
randalthor 0:9646fc4e3fae 158 driveLeft = cmd_velocity_linx - (0.17 * cmd_velocity_angz * 0.5);
randalthor 0:9646fc4e3fae 159 driveRight = cmd_velocity_linx + (0.17 * cmd_velocity_angz * 0.5);
randalthor 0:9646fc4e3fae 160
randalthor 0:9646fc4e3fae 161 errorLeft = driveLeft - linearLeft;
randalthor 0:9646fc4e3fae 162 errorRight = driveRight - linearRight;
randalthor 0:9646fc4e3fae 163
randalthor 0:9646fc4e3fae 164 iRight = iRight + (errorRight * 1);
randalthor 0:9646fc4e3fae 165 iLeft = iLeft + (errorLeft * 1);
randalthor 0:9646fc4e3fae 166 dLeft = (errorLeft - errorLeft_1) * 200;
randalthor 0:9646fc4e3fae 167 dRight = (errorRight - errorRight_1) * 200;
randalthor 0:9646fc4e3fae 168
randalthor 0:9646fc4e3fae 169 errorLeft_1 = errorLeft;
randalthor 0:9646fc4e3fae 170 errorRight_1 = errorRight;
randalthor 0:9646fc4e3fae 171
randalthor 0:9646fc4e3fae 172 if(iRight > i_max) iRight = i_max;
randalthor 0:9646fc4e3fae 173 if(iRight < -i_max) iRight = i_max;
randalthor 0:9646fc4e3fae 174 if(iLeft > i_max) iLeft = i_max;
randalthor 0:9646fc4e3fae 175 if(iLeft < -i_max) iLeft = i_max;
randalthor 0:9646fc4e3fae 176
randalthor 0:9646fc4e3fae 177 pwmLeft = kp_left * errorLeft + ki_left * iLeft + kd_left * dLeft;
randalthor 0:9646fc4e3fae 178 pwmRight = kp_right * errorRight + ki_right * iRight + kd_right * dRight;
randalthor 0:9646fc4e3fae 179 //HAL_GPIO_TogglePin(GPIOB, GPIO_PIN_0);
randalthor 0:9646fc4e3fae 180
randalthor 0:9646fc4e3fae 181
randalthor 0:9646fc4e3fae 182 if(cmd_velocity_linx || cmd_velocity_angz)
randalthor 0:9646fc4e3fae 183 {
randalthor 0:9646fc4e3fae 184
randalthor 0:9646fc4e3fae 185
randalthor 0:9646fc4e3fae 186
randalthor 0:9646fc4e3fae 187
randalthor 0:9646fc4e3fae 188 if(pwmLeft>0)
randalthor 0:9646fc4e3fae 189 {
randalthor 0:9646fc4e3fae 190 HAL_GPIO_WritePin(GPIOD, GPIO_PIN_3, GPIO_PIN_SET);
randalthor 0:9646fc4e3fae 191 //wait(0.005);
randalthor 0:9646fc4e3fae 192 HAL_GPIO_WritePin(GPIOD, GPIO_PIN_4, GPIO_PIN_RESET);
randalthor 0:9646fc4e3fae 193 //wait(0.005);
randalthor 0:9646fc4e3fae 194 pwmLeft = 61 + pwmLeft;
randalthor 0:9646fc4e3fae 195 }
randalthor 0:9646fc4e3fae 196 if (pwmLeft<0)
randalthor 0:9646fc4e3fae 197 {
randalthor 0:9646fc4e3fae 198 HAL_GPIO_WritePin(GPIOD, GPIO_PIN_3, GPIO_PIN_RESET);
randalthor 0:9646fc4e3fae 199 // wait(0.005);
randalthor 0:9646fc4e3fae 200 HAL_GPIO_WritePin(GPIOD, GPIO_PIN_4, GPIO_PIN_SET);
randalthor 0:9646fc4e3fae 201 //wait(0.005);
randalthor 0:9646fc4e3fae 202 pwmLeft = 61 - pwmLeft;
randalthor 0:9646fc4e3fae 203 }
randalthor 0:9646fc4e3fae 204 if(pwmRight>0)
randalthor 0:9646fc4e3fae 205 {
randalthor 0:9646fc4e3fae 206 HAL_GPIO_WritePin(GPIOD, GPIO_PIN_14, GPIO_PIN_SET);
randalthor 0:9646fc4e3fae 207 //wait(0.005);
randalthor 0:9646fc4e3fae 208 HAL_GPIO_WritePin(GPIOD, GPIO_PIN_15, GPIO_PIN_RESET);
randalthor 0:9646fc4e3fae 209 //wait(0.005);
randalthor 0:9646fc4e3fae 210 pwmRight = 60 + pwmRight;
randalthor 0:9646fc4e3fae 211 }
randalthor 0:9646fc4e3fae 212 if(pwmRight<0)
randalthor 0:9646fc4e3fae 213 {
randalthor 0:9646fc4e3fae 214 HAL_GPIO_WritePin(GPIOD, GPIO_PIN_14, GPIO_PIN_RESET);
randalthor 0:9646fc4e3fae 215 //wait(0.005);
randalthor 0:9646fc4e3fae 216 HAL_GPIO_WritePin(GPIOD, GPIO_PIN_15, GPIO_PIN_SET);
randalthor 0:9646fc4e3fae 217 // wait(0.005);
randalthor 0:9646fc4e3fae 218 pwmRight = 60 - pwmRight;
randalthor 0:9646fc4e3fae 219 }
randalthor 0:9646fc4e3fae 220
randalthor 0:9646fc4e3fae 221
randalthor 0:9646fc4e3fae 222 }
randalthor 0:9646fc4e3fae 223 else
randalthor 0:9646fc4e3fae 224 {
randalthor 0:9646fc4e3fae 225 pwmRight = 0;
randalthor 0:9646fc4e3fae 226 pwmLeft = 0;
randalthor 0:9646fc4e3fae 227 iRight = 0;
randalthor 0:9646fc4e3fae 228 iLeft = 0;
randalthor 0:9646fc4e3fae 229 }
randalthor 0:9646fc4e3fae 230
randalthor 0:9646fc4e3fae 231 if(pwmRight > 99) pwmRight = 99;
randalthor 0:9646fc4e3fae 232 if(pwmRight < -99) pwmRight = -99;
randalthor 0:9646fc4e3fae 233 if(pwmLeft > 99) pwmLeft = 99;
randalthor 0:9646fc4e3fae 234 if(pwmLeft < -99) pwmLeft = -99;
randalthor 0:9646fc4e3fae 235
randalthor 0:9646fc4e3fae 236 __HAL_TIM_SetCompare(&htim9, TIM_CHANNEL_1, pwmLeft);
randalthor 0:9646fc4e3fae 237 __HAL_TIM_SetCompare(&htim9, TIM_CHANNEL_2, pwmRight);
randalthor 0:9646fc4e3fae 238
randalthor 0:9646fc4e3fae 239 //htim9.Instance->CCR1 = pwm1;
randalthor 0:9646fc4e3fae 240 // htim9.Instance->CCR2 = pwm2;
randalthor 0:9646fc4e3fae 241
randalthor 0:9646fc4e3fae 242
randalthor 0:9646fc4e3fae 243 osDelay(5);
randalthor 0:9646fc4e3fae 244
randalthor 0:9646fc4e3fae 245
randalthor 0:9646fc4e3fae 246 }
randalthor 0:9646fc4e3fae 247 }
randalthor 0:9646fc4e3fae 248
randalthor 0:9646fc4e3fae 249 osThreadDef(feedback_thread, osPriorityNormal, DEFAULT_STACK_SIZE);
randalthor 0:9646fc4e3fae 250 osThreadDef(velocity_control, osPriorityNormal, DEFAULT_STACK_SIZE);
randalthor 0:9646fc4e3fae 251
randalthor 0:9646fc4e3fae 252
randalthor 0:9646fc4e3fae 253 int main()
randalthor 0:9646fc4e3fae 254 {
randalthor 0:9646fc4e3fae 255
randalthor 0:9646fc4e3fae 256 /* Reset of all peripherals, Initializes the Flash interface and the Systick. */
randalthor 0:9646fc4e3fae 257 HAL_Init();
randalthor 0:9646fc4e3fae 258
randalthor 0:9646fc4e3fae 259
randalthor 0:9646fc4e3fae 260 /* Initialize all configured peripherals */
randalthor 0:9646fc4e3fae 261 MX_GPIO_Init();
randalthor 0:9646fc4e3fae 262 MX_DMA_Init();
randalthor 0:9646fc4e3fae 263 MX_ADC2_Init();
randalthor 0:9646fc4e3fae 264 MX_TIM1_Init();
randalthor 0:9646fc4e3fae 265 MX_TIM4_Init();
randalthor 0:9646fc4e3fae 266 MX_TIM9_Init();
randalthor 0:9646fc4e3fae 267
randalthor 0:9646fc4e3fae 268 /*Initialize necessery configuration*/
randalthor 0:9646fc4e3fae 269 HAL_ADC_Start_DMA(&hadc2, (uint32_t *)ADC_buffer, 2);
randalthor 0:9646fc4e3fae 270 HAL_TIM_Encoder_Start(&htim1, TIM_CHANNEL_1|TIM_CHANNEL_2);
randalthor 0:9646fc4e3fae 271 HAL_TIM_Encoder_Start(&htim4, TIM_CHANNEL_1|TIM_CHANNEL_2);
randalthor 0:9646fc4e3fae 272 HAL_TIM_PWM_Start(&htim9, TIM_CHANNEL_1);
randalthor 0:9646fc4e3fae 273 HAL_TIM_PWM_Start(&htim9, TIM_CHANNEL_2);
randalthor 0:9646fc4e3fae 274
randalthor 0:9646fc4e3fae 275 nh.initNode();
randalthor 0:9646fc4e3fae 276 nh.advertise(RobotFeedback);
randalthor 0:9646fc4e3fae 277 nh.subscribe(sub);
randalthor 0:9646fc4e3fae 278
randalthor 0:9646fc4e3fae 279 printf("\n\n*** RTOS basic example ***\n");
randalthor 0:9646fc4e3fae 280
randalthor 0:9646fc4e3fae 281
randalthor 0:9646fc4e3fae 282 osThreadCreate(osThread(feedback_thread), NULL);
randalthor 0:9646fc4e3fae 283 osThreadCreate(osThread(velocity_control), NULL);
randalthor 0:9646fc4e3fae 284
randalthor 0:9646fc4e3fae 285
randalthor 0:9646fc4e3fae 286 while (true) {
randalthor 0:9646fc4e3fae 287 led2 = !led2;
randalthor 0:9646fc4e3fae 288 osDelay(500);
randalthor 0:9646fc4e3fae 289 }
randalthor 0:9646fc4e3fae 290 }
randalthor 0:9646fc4e3fae 291
randalthor 0:9646fc4e3fae 292 /* TIM1 init function */
randalthor 0:9646fc4e3fae 293 static void MX_TIM1_Init(void)
randalthor 0:9646fc4e3fae 294 {
randalthor 0:9646fc4e3fae 295
randalthor 0:9646fc4e3fae 296 TIM_Encoder_InitTypeDef sConfig;
randalthor 0:9646fc4e3fae 297 TIM_MasterConfigTypeDef sMasterConfig;
randalthor 0:9646fc4e3fae 298
randalthor 0:9646fc4e3fae 299 htim1.Instance = TIM1;
randalthor 0:9646fc4e3fae 300 htim1.Init.Prescaler = 0;
randalthor 0:9646fc4e3fae 301 htim1.Init.CounterMode = TIM_COUNTERMODE_UP;
randalthor 0:9646fc4e3fae 302 htim1.Init.Period = 65535;
randalthor 0:9646fc4e3fae 303 htim1.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
randalthor 0:9646fc4e3fae 304 htim1.Init.RepetitionCounter = 0;
randalthor 0:9646fc4e3fae 305 sConfig.EncoderMode = TIM_ENCODERMODE_TI12;
randalthor 0:9646fc4e3fae 306 sConfig.IC1Polarity = TIM_ICPOLARITY_RISING;
randalthor 0:9646fc4e3fae 307 sConfig.IC1Selection = TIM_ICSELECTION_DIRECTTI;
randalthor 0:9646fc4e3fae 308 sConfig.IC1Prescaler = TIM_ICPSC_DIV1;
randalthor 0:9646fc4e3fae 309 sConfig.IC1Filter = 0;
randalthor 0:9646fc4e3fae 310 sConfig.IC2Polarity = TIM_ICPOLARITY_RISING;
randalthor 0:9646fc4e3fae 311 sConfig.IC2Selection = TIM_ICSELECTION_DIRECTTI;
randalthor 0:9646fc4e3fae 312 sConfig.IC2Prescaler = TIM_ICPSC_DIV1;
randalthor 0:9646fc4e3fae 313 sConfig.IC2Filter = 0;
randalthor 0:9646fc4e3fae 314 if (HAL_TIM_Encoder_Init(&htim1, &sConfig) != HAL_OK)
randalthor 0:9646fc4e3fae 315 {
randalthor 0:9646fc4e3fae 316 Error_Handler();
randalthor 0:9646fc4e3fae 317 }
randalthor 0:9646fc4e3fae 318
randalthor 0:9646fc4e3fae 319 sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
randalthor 0:9646fc4e3fae 320 sMasterConfig.MasterOutputTrigger2 = TIM_TRGO2_RESET;
randalthor 0:9646fc4e3fae 321 sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
randalthor 0:9646fc4e3fae 322 if (HAL_TIMEx_MasterConfigSynchronization(&htim1, &sMasterConfig) != HAL_OK)
randalthor 0:9646fc4e3fae 323 {
randalthor 0:9646fc4e3fae 324 Error_Handler();
randalthor 0:9646fc4e3fae 325 }
randalthor 0:9646fc4e3fae 326
randalthor 0:9646fc4e3fae 327 }
randalthor 0:9646fc4e3fae 328
randalthor 0:9646fc4e3fae 329 /* TIM4 init function */
randalthor 0:9646fc4e3fae 330 static void MX_TIM4_Init(void)
randalthor 0:9646fc4e3fae 331 {
randalthor 0:9646fc4e3fae 332
randalthor 0:9646fc4e3fae 333 TIM_Encoder_InitTypeDef sConfig;
randalthor 0:9646fc4e3fae 334 TIM_MasterConfigTypeDef sMasterConfig;
randalthor 0:9646fc4e3fae 335
randalthor 0:9646fc4e3fae 336 htim4.Instance = TIM4;
randalthor 0:9646fc4e3fae 337 htim4.Init.Prescaler = 0;
randalthor 0:9646fc4e3fae 338 htim4.Init.CounterMode = TIM_COUNTERMODE_UP;
randalthor 0:9646fc4e3fae 339 htim4.Init.Period = 65535;
randalthor 0:9646fc4e3fae 340 htim4.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
randalthor 0:9646fc4e3fae 341 sConfig.EncoderMode = TIM_ENCODERMODE_TI12;
randalthor 0:9646fc4e3fae 342 sConfig.IC1Polarity = TIM_ICPOLARITY_RISING;
randalthor 0:9646fc4e3fae 343 sConfig.IC1Selection = TIM_ICSELECTION_DIRECTTI;
randalthor 0:9646fc4e3fae 344 sConfig.IC1Prescaler = TIM_ICPSC_DIV1;
randalthor 0:9646fc4e3fae 345 sConfig.IC1Filter = 0;
randalthor 0:9646fc4e3fae 346 sConfig.IC2Polarity = TIM_ICPOLARITY_RISING;
randalthor 0:9646fc4e3fae 347 sConfig.IC2Selection = TIM_ICSELECTION_DIRECTTI;
randalthor 0:9646fc4e3fae 348 sConfig.IC2Prescaler = TIM_ICPSC_DIV1;
randalthor 0:9646fc4e3fae 349 sConfig.IC2Filter = 0;
randalthor 0:9646fc4e3fae 350 if (HAL_TIM_Encoder_Init(&htim4, &sConfig) != HAL_OK)
randalthor 0:9646fc4e3fae 351 {
randalthor 0:9646fc4e3fae 352 Error_Handler();
randalthor 0:9646fc4e3fae 353 }
randalthor 0:9646fc4e3fae 354
randalthor 0:9646fc4e3fae 355 sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
randalthor 0:9646fc4e3fae 356 sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
randalthor 0:9646fc4e3fae 357 if (HAL_TIMEx_MasterConfigSynchronization(&htim4, &sMasterConfig) != HAL_OK)
randalthor 0:9646fc4e3fae 358 {
randalthor 0:9646fc4e3fae 359 Error_Handler();
randalthor 0:9646fc4e3fae 360 }
randalthor 0:9646fc4e3fae 361
randalthor 0:9646fc4e3fae 362 }
randalthor 0:9646fc4e3fae 363
randalthor 0:9646fc4e3fae 364 /* ADC2 init function */
randalthor 0:9646fc4e3fae 365 static void MX_ADC2_Init(void)
randalthor 0:9646fc4e3fae 366 {
randalthor 0:9646fc4e3fae 367
randalthor 0:9646fc4e3fae 368 ADC_ChannelConfTypeDef sConfig;
randalthor 0:9646fc4e3fae 369
randalthor 0:9646fc4e3fae 370 /**Configure the global features of the ADC (Clock, Resolution, Data Alignment and number of conversion)
randalthor 0:9646fc4e3fae 371 */
randalthor 0:9646fc4e3fae 372 hadc2.Instance = ADC2;
randalthor 0:9646fc4e3fae 373 hadc2.Init.ClockPrescaler = ADC_CLOCK_SYNC_PCLK_DIV2;
randalthor 0:9646fc4e3fae 374 hadc2.Init.Resolution = ADC_RESOLUTION_8B;
randalthor 0:9646fc4e3fae 375 hadc2.Init.ScanConvMode = ENABLE;
randalthor 0:9646fc4e3fae 376 hadc2.Init.ContinuousConvMode = ENABLE;
randalthor 0:9646fc4e3fae 377 hadc2.Init.DiscontinuousConvMode = DISABLE;
randalthor 0:9646fc4e3fae 378 hadc2.Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_NONE;
randalthor 0:9646fc4e3fae 379 hadc2.Init.ExternalTrigConv = ADC_SOFTWARE_START;
randalthor 0:9646fc4e3fae 380 hadc2.Init.DataAlign = ADC_DATAALIGN_RIGHT;
randalthor 0:9646fc4e3fae 381 hadc2.Init.NbrOfConversion = 2;
randalthor 0:9646fc4e3fae 382 hadc2.Init.DMAContinuousRequests = ENABLE;
randalthor 0:9646fc4e3fae 383 hadc2.Init.EOCSelection = ADC_EOC_SINGLE_CONV;
randalthor 0:9646fc4e3fae 384 if (HAL_ADC_Init(&hadc2) != HAL_OK)
randalthor 0:9646fc4e3fae 385 {
randalthor 0:9646fc4e3fae 386 Error_Handler();
randalthor 0:9646fc4e3fae 387 }
randalthor 0:9646fc4e3fae 388
randalthor 0:9646fc4e3fae 389 /**Configure for the selected ADC regular channel its corresponding rank in the sequencer and its sample time.
randalthor 0:9646fc4e3fae 390 */
randalthor 0:9646fc4e3fae 391 sConfig.Channel = ADC_CHANNEL_5;
randalthor 0:9646fc4e3fae 392 sConfig.Rank = 1;
randalthor 0:9646fc4e3fae 393 sConfig.SamplingTime = ADC_SAMPLETIME_144CYCLES;
randalthor 0:9646fc4e3fae 394 if (HAL_ADC_ConfigChannel(&hadc2, &sConfig) != HAL_OK)
randalthor 0:9646fc4e3fae 395 {
randalthor 0:9646fc4e3fae 396 Error_Handler();
randalthor 0:9646fc4e3fae 397 }
randalthor 0:9646fc4e3fae 398
randalthor 0:9646fc4e3fae 399 /**Configure for the selected ADC regular channel its corresponding rank in the sequencer and its sample time.
randalthor 0:9646fc4e3fae 400 */
randalthor 0:9646fc4e3fae 401 sConfig.Channel = ADC_CHANNEL_6;
randalthor 0:9646fc4e3fae 402 sConfig.Rank = 2;
randalthor 0:9646fc4e3fae 403 if (HAL_ADC_ConfigChannel(&hadc2, &sConfig) != HAL_OK)
randalthor 0:9646fc4e3fae 404 {
randalthor 0:9646fc4e3fae 405 Error_Handler();
randalthor 0:9646fc4e3fae 406 }
randalthor 0:9646fc4e3fae 407
randalthor 0:9646fc4e3fae 408 }
randalthor 0:9646fc4e3fae 409
randalthor 0:9646fc4e3fae 410 /* TIM9 init function */
randalthor 0:9646fc4e3fae 411 static void MX_TIM9_Init(void)
randalthor 0:9646fc4e3fae 412 {
randalthor 0:9646fc4e3fae 413
randalthor 0:9646fc4e3fae 414 TIM_OC_InitTypeDef sConfigOC;
randalthor 0:9646fc4e3fae 415
randalthor 0:9646fc4e3fae 416 htim9.Instance = TIM9;
randalthor 0:9646fc4e3fae 417 htim9.Init.Prescaler = 215;
randalthor 0:9646fc4e3fae 418 htim9.Init.CounterMode = TIM_COUNTERMODE_UP;
randalthor 0:9646fc4e3fae 419 htim9.Init.Period = 99;
randalthor 0:9646fc4e3fae 420 htim9.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
randalthor 0:9646fc4e3fae 421 if (HAL_TIM_PWM_Init(&htim9) != HAL_OK)
randalthor 0:9646fc4e3fae 422 {
randalthor 0:9646fc4e3fae 423 Error_Handler();
randalthor 0:9646fc4e3fae 424 }
randalthor 0:9646fc4e3fae 425
randalthor 0:9646fc4e3fae 426 sConfigOC.OCMode = TIM_OCMODE_PWM1;
randalthor 0:9646fc4e3fae 427 sConfigOC.Pulse = 0;
randalthor 0:9646fc4e3fae 428 sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH;
randalthor 0:9646fc4e3fae 429 sConfigOC.OCFastMode = TIM_OCFAST_DISABLE;
randalthor 0:9646fc4e3fae 430 if (HAL_TIM_PWM_ConfigChannel(&htim9, &sConfigOC, TIM_CHANNEL_1) != HAL_OK)
randalthor 0:9646fc4e3fae 431 {
randalthor 0:9646fc4e3fae 432 Error_Handler();
randalthor 0:9646fc4e3fae 433 }
randalthor 0:9646fc4e3fae 434
randalthor 0:9646fc4e3fae 435 if (HAL_TIM_PWM_ConfigChannel(&htim9, &sConfigOC, TIM_CHANNEL_2) != HAL_OK)
randalthor 0:9646fc4e3fae 436 {
randalthor 0:9646fc4e3fae 437 Error_Handler();
randalthor 0:9646fc4e3fae 438 }
randalthor 0:9646fc4e3fae 439
randalthor 0:9646fc4e3fae 440 HAL_TIM_MspPostInit(&htim9);
randalthor 0:9646fc4e3fae 441
randalthor 0:9646fc4e3fae 442 }
randalthor 0:9646fc4e3fae 443
randalthor 0:9646fc4e3fae 444 /**
randalthor 0:9646fc4e3fae 445 * Enable DMA controller clock
randalthor 0:9646fc4e3fae 446 */
randalthor 0:9646fc4e3fae 447 static void MX_DMA_Init(void)
randalthor 0:9646fc4e3fae 448 {
randalthor 0:9646fc4e3fae 449 /* DMA controller clock enable */
randalthor 0:9646fc4e3fae 450 __HAL_RCC_DMA2_CLK_ENABLE();
randalthor 0:9646fc4e3fae 451
randalthor 0:9646fc4e3fae 452 /* DMA interrupt init */
randalthor 0:9646fc4e3fae 453 /* DMA2_Stream2_IRQn interrupt configuration */
randalthor 0:9646fc4e3fae 454 HAL_NVIC_SetPriority(DMA2_Stream2_IRQn, 0, 0);
randalthor 0:9646fc4e3fae 455 HAL_NVIC_EnableIRQ(DMA2_Stream2_IRQn);
randalthor 0:9646fc4e3fae 456
randalthor 0:9646fc4e3fae 457 }
randalthor 0:9646fc4e3fae 458
randalthor 0:9646fc4e3fae 459 /** Configure pins as
randalthor 0:9646fc4e3fae 460 * Analog
randalthor 0:9646fc4e3fae 461 * Input
randalthor 0:9646fc4e3fae 462 * Output
randalthor 0:9646fc4e3fae 463 * EVENT_OUT
randalthor 0:9646fc4e3fae 464 * EXTI
randalthor 0:9646fc4e3fae 465 */
randalthor 0:9646fc4e3fae 466 static void MX_GPIO_Init(void)
randalthor 0:9646fc4e3fae 467 {
randalthor 0:9646fc4e3fae 468
randalthor 0:9646fc4e3fae 469 GPIO_InitTypeDef GPIO_InitStruct;
randalthor 0:9646fc4e3fae 470
randalthor 0:9646fc4e3fae 471 /* GPIO Ports Clock Enable */
randalthor 0:9646fc4e3fae 472 __HAL_RCC_GPIOE_CLK_ENABLE();
randalthor 0:9646fc4e3fae 473 __HAL_RCC_GPIOA_CLK_ENABLE();
randalthor 0:9646fc4e3fae 474 __HAL_RCC_GPIOB_CLK_ENABLE();
randalthor 0:9646fc4e3fae 475 __HAL_RCC_GPIOD_CLK_ENABLE();
randalthor 0:9646fc4e3fae 476
randalthor 0:9646fc4e3fae 477 /*Configure GPIO pin Output Level */
randalthor 0:9646fc4e3fae 478 HAL_GPIO_WritePin(GPIOB, GPIO_PIN_0, GPIO_PIN_RESET);
randalthor 0:9646fc4e3fae 479
randalthor 0:9646fc4e3fae 480 /*Configure GPIO pin Output Level */
randalthor 0:9646fc4e3fae 481 HAL_GPIO_WritePin(GPIOD, GPIO_PIN_14|GPIO_PIN_15|GPIO_PIN_3|GPIO_PIN_4, GPIO_PIN_RESET);
randalthor 0:9646fc4e3fae 482
randalthor 0:9646fc4e3fae 483 /*Configure GPIO pin : PB0 */
randalthor 0:9646fc4e3fae 484 GPIO_InitStruct.Pin = GPIO_PIN_0;
randalthor 0:9646fc4e3fae 485 GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
randalthor 0:9646fc4e3fae 486 GPIO_InitStruct.Pull = GPIO_NOPULL;
randalthor 0:9646fc4e3fae 487 GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
randalthor 0:9646fc4e3fae 488 HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
randalthor 0:9646fc4e3fae 489
randalthor 0:9646fc4e3fae 490 /*Configure GPIO pins : PD14 PD15 PD3 PD4 */
randalthor 0:9646fc4e3fae 491 GPIO_InitStruct.Pin = GPIO_PIN_14|GPIO_PIN_15|GPIO_PIN_3|GPIO_PIN_4;
randalthor 0:9646fc4e3fae 492 GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
randalthor 0:9646fc4e3fae 493 GPIO_InitStruct.Pull = GPIO_NOPULL;
randalthor 0:9646fc4e3fae 494 GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
randalthor 0:9646fc4e3fae 495 HAL_GPIO_Init(GPIOD, &GPIO_InitStruct);
randalthor 0:9646fc4e3fae 496
randalthor 0:9646fc4e3fae 497 }
randalthor 0:9646fc4e3fae 498 /* USER CODE BEGIN 4 */
randalthor 0:9646fc4e3fae 499
randalthor 0:9646fc4e3fae 500 /* USER CODE END 4 */
randalthor 0:9646fc4e3fae 501
randalthor 0:9646fc4e3fae 502 /**
randalthor 0:9646fc4e3fae 503 * @brief This function is executed in case of error occurrence.
randalthor 0:9646fc4e3fae 504 * @param None
randalthor 0:9646fc4e3fae 505 * @retval None
randalthor 0:9646fc4e3fae 506 */
randalthor 0:9646fc4e3fae 507 void Error_Handler(void)
randalthor 0:9646fc4e3fae 508 {
randalthor 0:9646fc4e3fae 509 /* USER CODE BEGIN Error_Handler */
randalthor 0:9646fc4e3fae 510 /* User can add his own implementation to report the HAL error return state */
randalthor 0:9646fc4e3fae 511 while(1)
randalthor 0:9646fc4e3fae 512 {
randalthor 0:9646fc4e3fae 513 }
randalthor 0:9646fc4e3fae 514 /* USER CODE END Error_Handler */
randalthor 0:9646fc4e3fae 515 }
randalthor 0:9646fc4e3fae 516
randalthor 0:9646fc4e3fae 517 void HAL_TIM_MspPostInit(TIM_HandleTypeDef* htim)
randalthor 0:9646fc4e3fae 518 {
randalthor 0:9646fc4e3fae 519
randalthor 0:9646fc4e3fae 520 GPIO_InitTypeDef GPIO_InitStruct;
randalthor 0:9646fc4e3fae 521 if(htim->Instance==TIM9)
randalthor 0:9646fc4e3fae 522 {
randalthor 0:9646fc4e3fae 523 /* USER CODE BEGIN TIM9_MspPostInit 0 */
randalthor 0:9646fc4e3fae 524
randalthor 0:9646fc4e3fae 525 /* USER CODE END TIM9_MspPostInit 0 */
randalthor 0:9646fc4e3fae 526
randalthor 0:9646fc4e3fae 527 /**TIM9 GPIO Configuration
randalthor 0:9646fc4e3fae 528 PE5 ------> TIM9_CH1
randalthor 0:9646fc4e3fae 529 PE6 ------> TIM9_CH2
randalthor 0:9646fc4e3fae 530 */
randalthor 0:9646fc4e3fae 531 GPIO_InitStruct.Pin = GPIO_PIN_5|GPIO_PIN_6;
randalthor 0:9646fc4e3fae 532 GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
randalthor 0:9646fc4e3fae 533 GPIO_InitStruct.Pull = GPIO_NOPULL;
randalthor 0:9646fc4e3fae 534 GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
randalthor 0:9646fc4e3fae 535 GPIO_InitStruct.Alternate = GPIO_AF3_TIM9;
randalthor 0:9646fc4e3fae 536 HAL_GPIO_Init(GPIOE, &GPIO_InitStruct);
randalthor 0:9646fc4e3fae 537
randalthor 0:9646fc4e3fae 538 /* USER CODE BEGIN TIM9_MspPostInit 1 */
randalthor 0:9646fc4e3fae 539
randalthor 0:9646fc4e3fae 540 /* USER CODE END TIM9_MspPostInit 1 */
randalthor 0:9646fc4e3fae 541 }
randalthor 0:9646fc4e3fae 542
randalthor 0:9646fc4e3fae 543 }