Revision 1:0f99619433e0, committed 2017-03-04

Comitter:

randalthor

Date:

Sat Mar 04 14:08:19 2017 +0000

Parent:

0:4d2d2219c8e4

Commit message:

fatihh

Changed in this revision

diff -r 4d2d2219c8e4 -r 0f99619433e0 main.cpp
--- a/main.cpp	Mon Jan 30 09:21:03 2017 +0000
+++ b/main.cpp	Sat Mar 04 14:08:19 2017 +0000
@@ -1,59 +1,67 @@
 #include "mbed.h"
-#include "rtos.h"
+#include "cmsis_os.h"
 #include "main.h"
 #include "stm32f7xx_hal.h"
 #include <ros.h>
 #include <geometry_msgs/Twist.h>
 #include <geometry_msgs/Vector3.h>
-#include <beginner_tutorials/mobileRobot.h>
+#include <openlab/mobileRobot.h>
 #include <std_msgs/String.h>
 #include <std_msgs/Float32.h>
 #include <std_msgs/Bool.h>
 
-#define pi 3.14159  //for angular velocity calculation
+#define pi 3.1416  //for angular velocity calculation
 #define length 0.3 //distance between 2 wheel - meter
 #define radius 0.036 //radius of wheel with load - meter 
 #define timeConst 0.00926 //time constant
 
 /*Global variables*/
-uint8_t ADC_buffer[2];
+int8_t ADC_buffer[2];
 int32_t encoder1_1 = 0;
 int32_t encoder1=0;
 int32_t encoder2_1 = 0;
 int32_t encoder2=0;
-float pwm1=0;
-float pwm2=0;
+float pwmLeft=0;
+float pwmRight=0;
 float dc_current1=0;
 float dc_current2=0;
-float real_velocity_linx=0;
-float real_velocity_angz=0;
-float cmd_velocity_linx=0;
-float cmd_velocity_angz=0;
-float real_velocity1=0;
-float real_velocity2=0;
-float kp_lin = 10000;
-float kd_lin = 1;
-float ki_lin = 1;
-float kp_ang = 5000;
-float kd_ang = 1;
-float ki_ang = 1;
-float errDirVel_1 = 0;
-float errAngVel_1 = 0;
-float errDirVel = 0;
-float errAngVel = 0;
-float integralDirVel = 0;
-float integralDirVel_max = 50;
-float integralAngVel = 0;
-float integralAngVel_max = 50;
-float derivativeDirVel = 0;
-float derivativeAngVel = 0;
-float comDirVel = 0;
-float comAngVel = 0;
+double real_velocity_linx=0;
+double real_velocity_angz=0;
+double cmd_velocity_linx=0;
+double cmd_velocity_angz=0;
+
+float kp_left = 39;
+float kd_left = 2;
+float ki_left = 10;
+float kp_right = 39;
+float kd_right = 2;
+float ki_right = 10;
+double errDirVel_1 = 0;
+double errAngVel_1 = 0;
+double errorLeft = 0;
+double errorRight = 0;
+double errorLeft_1 = 0;
+double errorRight_1 = 0;
+double linearLeft = 0;
+double linearRight = 0;
+double driveLeft = 0;
+double driveRight = 0;
+double iLeft = 0;
+double iRight = 0;
+
+float i_max = 1000;
+float dLeft = 0;
+float dRight = 0;
+
+int diff1=0;
+int diff2=0;
+float current1;
+float current2;
 
 
 
 ros::NodeHandle  nh;
-beginner_tutorials::mobileRobot str_msg;
+openlab::mobileRobot str_msg;
 ros::Publisher RobotFeedback("RobotFeedback", &str_msg);
 
 void callback_vel( const geometry_msgs::Twist& vel){
@@ -63,14 +71,15 @@
  
   
    nh.spinOnce();
-   Thread::wait(10);
+   osDelay(10);
 }
 
 ros::Subscriber<geometry_msgs::Twist> sub("cmd_vel", callback_vel );
 
 /*loop control*/
-DigitalOut led1(LED1);
+//DigitalOut led1(LED1);
 DigitalOut led2(LED2);
+DigitalOut led3(LED3);
 /* USER CODE BEGIN Includes */
 
 /* USER CODE END Includes */
@@ -96,114 +105,150 @@
 
 void HAL_TIM_MspPostInit(TIM_HandleTypeDef *htim);
 
-void feedback_thread(void const *argument)
+void feedback_thread(void const *args)
 {
     while (true) {
      
       
-      str_msg.pwm_1 = pwm1;
-      str_msg.pwm_2 = pwm2;
+      str_msg.pwm_1 = pwmLeft;
+      str_msg.pwm_2 = pwmRight;
       str_msg.real_velocity_linx = real_velocity_linx;
       str_msg.real_velocity_angz = real_velocity_angz;
       str_msg.cmd_velocity_linx = cmd_velocity_linx;
       str_msg.cmd_velocity_angz = cmd_velocity_angz;
-      str_msg.dc_current_1 = ADC_buffer[0];
-      str_msg.dc_current_2 = ADC_buffer[1];
+      str_msg.dc_current_1 = current1;
+      str_msg.dc_current_2 = current2;
       str_msg.encoder_1 = encoder1;
       str_msg.encoder_2 = encoder2;
       RobotFeedback.publish( &str_msg );
       nh.spinOnce();
-      Thread::wait(10);
-        
-    }
-}
-
-void velocity_calculations(void const *argument)
-{
-    while (true) {
-
-
-    encoder1=TIM1->CNT;
-    encoder2=TIM4->CNT;
-
-
-    real_velocity1 = (encoder1 - encoder1_1) * 0.000747998*108;  //left wheel angular velovity 2pi/8400 times 
-    real_velocity2 = (encoder2 - encoder2_1) * 0.000747998*108;  //right wheel angular velocity
-
-    real_velocity_angz = (real_velocity2 - real_velocity1);
-    real_velocity_linx = (real_velocity2 + real_velocity1) * 0.5f * radius;  // v = w.r
-     
-    
-    encoder1_1 = encoder1;
-    encoder2_1 = encoder2;
-
-    //__HAL_TIM_SetCounter(&htim1, 0);
-    //__HAL_TIM_SetCounter(&htim3, 0);
-
-      Thread::wait(10);
+     osDelay(10);
         
     }
 }
 
-void motor_drive(void const *argument)
+void velocity_control(void const *args)
 {
+    
     while (true) {
 
-
-    errDirVel = cmd_velocity_linx - real_velocity_linx;
-    errAngVel = cmd_velocity_angz - real_velocity_angz;
+    current1 = ADC_buffer[0];
+    current2 = ADC_buffer[1];
+    encoder1 = TIM1->CNT;
+    encoder2 = TIM4->CNT;
+   
 
-    
-    integralDirVel = integralDirVel + (errDirVel * timeConst);
-    integralAngVel = integralAngVel + (errAngVel * timeConst);
-    derivativeDirVel = (errDirVel - errDirVel_1) / timeConst;
-    derivativeAngVel = (errAngVel - errAngVel_1) / timeConst;
+    diff1=encoder1 - encoder1_1;
+    diff2=encoder2 - encoder2_1;
     
-    if(integralDirVel > integralDirVel_max) integralDirVel = integralDirVel_max;
-    if(integralDirVel < -integralDirVel_max) integralDirVel = -integralDirVel_max;
-    if(integralAngVel > integralAngVel_max) integralAngVel = integralAngVel_max;
-    if(integralAngVel < -integralAngVel_max) integralAngVel = -integralAngVel_max;
+    if(diff1 < - 30000) diff1 = diff1 + 65536;
+    if(diff1 > 30000)  diff1 = diff1 - 65536;
+    if(diff2 < - 30000) diff2 = diff2 + 65536;
+    if(diff2> 30000)  diff2 = diff2 - 65536;
+
+    linearLeft = diff1 * 0.000748*200*radius; 
+    linearRight = diff2 * 0.000748*200*radius; 
+ 
+    real_velocity_linx = (linearLeft + linearRight) * 0.5;
+    real_velocity_angz = (linearRight - linearLeft) * 5.8824;
+
+    encoder1_1 = encoder1;
+    encoder2_1 = encoder2;
+
+    driveLeft = cmd_velocity_linx - (0.17 * cmd_velocity_angz * 0.5);
+    driveRight = cmd_velocity_linx + (0.17 * cmd_velocity_angz * 0.5);
+
+    errorLeft = driveLeft - linearLeft;
+    errorRight = driveRight - linearRight;
 
-    comDirVel = kp_lin*errDirVel + ki_lin * integralDirVel + kd_lin * derivativeDirVel;
-    comAngVel = kp_ang*errAngVel + ki_ang * integralAngVel + kd_ang * derivativeAngVel;
+    iRight = iRight + (errorRight * 1);
+    iLeft = iLeft + (errorLeft * 1);
+    dLeft = (errorLeft - errorLeft_1) * 200;
+    dRight = (errorRight - errorRight_1) * 200;
+
+    errorLeft_1 = errorLeft;
+    errorRight_1 = errorRight;
+    
+    if(iRight > i_max) iRight = i_max;
+    if(iRight < -i_max) iRight = i_max;
+    if(iLeft > i_max) iLeft = i_max;
+    if(iLeft < -i_max) iLeft = i_max;
 
-    pwm1 = comDirVel + comAngVel;
-    pwm2 = comDirVel - comAngVel;
+   pwmLeft = kp_left * errorLeft + ki_left * iLeft + kd_left * dLeft;
+   pwmRight = kp_right * errorRight + ki_right * iRight + kd_right * dRight;
+   //HAL_GPIO_TogglePin(GPIOB, GPIO_PIN_0);
+  
 
-    if(pwm1>0)
+    if(cmd_velocity_linx || cmd_velocity_angz) 
+  {
+
+   
+   
+
+    if(pwmLeft>0)
     {
         HAL_GPIO_WritePin(GPIOD, GPIO_PIN_3, GPIO_PIN_SET);
+        //wait(0.005);
         HAL_GPIO_WritePin(GPIOD, GPIO_PIN_4, GPIO_PIN_RESET);
+        //wait(0.005);
+        pwmLeft = 61 + pwmLeft;
     }
-    else
+    if (pwmLeft<0)
     {
         HAL_GPIO_WritePin(GPIOD, GPIO_PIN_3, GPIO_PIN_RESET);
+       // wait(0.005);
         HAL_GPIO_WritePin(GPIOD, GPIO_PIN_4, GPIO_PIN_SET);
-        pwm1 = -pwm1;
+        //wait(0.005);
+        pwmLeft = 61 - pwmLeft;
     }
-    if(pwm2>0)
+    if(pwmRight>0)
     {
         HAL_GPIO_WritePin(GPIOD, GPIO_PIN_14, GPIO_PIN_SET);
+        //wait(0.005);
         HAL_GPIO_WritePin(GPIOD, GPIO_PIN_15, GPIO_PIN_RESET);
+        //wait(0.005);
+        pwmRight = 60 + pwmRight;
     }
-    else
+     if(pwmRight<0)
     {
         HAL_GPIO_WritePin(GPIOD, GPIO_PIN_14, GPIO_PIN_RESET);
+        //wait(0.005);
         HAL_GPIO_WritePin(GPIOD, GPIO_PIN_15, GPIO_PIN_SET);
-        pwm2 = -pwm2;
+       // wait(0.005);
+        pwmRight = 60 - pwmRight;
     }
-    __HAL_TIM_SetCompare(&htim9, TIM_CHANNEL_1, (int)pwm1);
-    __HAL_TIM_SetCompare(&htim9, TIM_CHANNEL_2, (int)pwm2);
+    
+    
+  }
+  else
+  {
+    pwmRight = 0;
+    pwmLeft = 0;
+    iRight = 0;
+    iLeft = 0;
+  }
 
-    errAngVel_1 = errAngVel;
-    errDirVel_1 = errDirVel;
+    if(pwmRight > 99) pwmRight = 99;
+    if(pwmRight < -99) pwmRight = -99;
+    if(pwmLeft > 99) pwmLeft = 99;
+    if(pwmLeft < -99) pwmLeft = -99;
+    
+   __HAL_TIM_SetCompare(&htim9, TIM_CHANNEL_1, pwmLeft);
+   __HAL_TIM_SetCompare(&htim9, TIM_CHANNEL_2, pwmRight);
+  
+    //htim9.Instance->CCR1 = pwm1;
+   // htim9.Instance->CCR2 = pwm2;
 
 
-      Thread::wait(10);
+      osDelay(5);
+
         
     }
 }
 
+ osThreadDef(feedback_thread, osPriorityNormal, DEFAULT_STACK_SIZE);
+ osThreadDef(velocity_control, osPriorityNormal, DEFAULT_STACK_SIZE);
+
 
 int main()
 {
@@ -233,14 +278,14 @@
 
     printf("\n\n*** RTOS basic example ***\n");
     
-    Thread thread(feedback_thread, NULL, osPriorityNormal, DEFAULT_STACK_SIZE);
-    Thread thread2(velocity_calculations, NULL, osPriorityNormal, DEFAULT_STACK_SIZE);
-    Thread thread3(motor_drive, NULL, osPriorityNormal, DEFAULT_STACK_SIZE);
+    
+   osThreadCreate(osThread(feedback_thread), NULL);
+   osThreadCreate(osThread(velocity_control), NULL);
     
 
     while (true) {
-        led1 = !led1;
-        Thread::wait(500);
+        led2 = !led2;
+        osDelay(500);
     }
 }
 
@@ -369,9 +414,9 @@
   TIM_OC_InitTypeDef sConfigOC;
 
   htim9.Instance = TIM9;
-  htim9.Init.Prescaler = 0;
+  htim9.Init.Prescaler = 215;
   htim9.Init.CounterMode = TIM_COUNTERMODE_UP;
-  htim9.Init.Period = 10800-1;
+  htim9.Init.Period = 99;
   htim9.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
   if (HAL_TIM_PWM_Init(&htim9) != HAL_OK)
   {
@@ -426,11 +471,22 @@
   /* GPIO Ports Clock Enable */
   __HAL_RCC_GPIOE_CLK_ENABLE();
   __HAL_RCC_GPIOA_CLK_ENABLE();
+  __HAL_RCC_GPIOB_CLK_ENABLE();
   __HAL_RCC_GPIOD_CLK_ENABLE();
 
   /*Configure GPIO pin Output Level */
+  HAL_GPIO_WritePin(GPIOB, GPIO_PIN_0, GPIO_PIN_RESET);
+
+  /*Configure GPIO pin Output Level */
   HAL_GPIO_WritePin(GPIOD, GPIO_PIN_14|GPIO_PIN_15|GPIO_PIN_3|GPIO_PIN_4, GPIO_PIN_RESET);
 
+  /*Configure GPIO pin : PB0 */
+  GPIO_InitStruct.Pin = GPIO_PIN_0;
+  GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
+  GPIO_InitStruct.Pull = GPIO_NOPULL;
+  GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
+  HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
+
   /*Configure GPIO pins : PD14 PD15 PD3 PD4 */
   GPIO_InitStruct.Pin = GPIO_PIN_14|GPIO_PIN_15|GPIO_PIN_3|GPIO_PIN_4;
   GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
@@ -439,7 +495,6 @@
   HAL_GPIO_Init(GPIOD, &GPIO_InitStruct);
 
 }
-
 /* USER CODE BEGIN 4 */
 
 /* USER CODE END 4 */

diff -r 4d2d2219c8e4 -r 0f99619433e0 ros_lib_indigo.lib
--- a/ros_lib_indigo.lib	Mon Jan 30 09:21:03 2017 +0000
+++ b/ros_lib_indigo.lib	Sat Mar 04 14:08:19 2017 +0000
@@ -1,1 +1,1 @@
-https://developer.mbed.org/users/randalthor/code/ros_lib_indigo/#ee325a31eec4
+https://developer.mbed.org/users/randalthor/code/ros_lib_indigo/#80d9bee5079a