Merge pull request #12 from iralabdisco/communication

Fixes encoder bug

Author: fdila

otto_controller/Core/Inc/communication_utils.h

@@ -30,3 +30,8 @@ typedef struct __attribute__((packed)){
   int right_ticks;
 } ticks_msg;
 
+typedef struct __attribute__((packed)){
+  float left_vel;
+  float right_vel;
+} wheel_msg;
+

otto_controller/Core/Inc/constants.h

@@ -4,11 +4,8 @@
 #define BASELINE 0.3 //distance between wheels in meters
 #define MAX_DUTY_CYCLE 790
 
-#define LEFT_TICKS_PER_METER 195788 //no
-#define RIGHT_TICKS_PER_METER 196829 //no
-
 #define TICKS_PER_REVOLUTION 148000 //x4 resolution
-#define RIGHT_WHEEL_CIRCUMFERENCE 0.8 //in meters
-#define LEFT_WHEEL_CIRCUMFERENCE  0.78 //in meters
+#define RIGHT_WHEEL_CIRCUMFERENCE 0.783 //in meters
+#define LEFT_WHEEL_CIRCUMFERENCE  0.789 //in meters
 
 #endif

otto_controller/Core/Inc/encoder.h

@@ -11,17 +11,13 @@ class Encoder {
   uint32_t current_millis_;
   int32_t ticks_;  //if negative the wheel is going backwards
   float wheel_circumference_;
-  int ticks_per_meter_;
 
   Encoder() {
     timer_ = NULL;
     wheel_circumference_ = 0;
-    ticks_per_meter_ = 0;
   }
 
-//  Encoder(TIM_HandleTypeDef *timer, float wheel_circ);
-  Encoder(TIM_HandleTypeDef *timer, int ticks_per_meters);
-
+  Encoder(TIM_HandleTypeDef *timer, float wheel_circ);
 
   void Setup();
 

otto_controller/Core/Src/encoder.cpp

@@ -1,14 +1,8 @@
 #include "encoder.h"
 
-//Encoder::Encoder(TIM_HandleTypeDef *timer, float wheel_circ) {
-//  timer_ = timer;
-//  wheel_circumference_ = wheel_circ;
-//
-//}
-
-Encoder::Encoder(TIM_HandleTypeDef *timer, int ticks_per_meter) {
+Encoder::Encoder(TIM_HandleTypeDef *timer, float wheel_circ) {
   timer_ = timer;
-  ticks_per_meter_ = ticks_per_meter;
+  wheel_circumference_ = wheel_circ;
 
 }
 
@@ -26,18 +20,12 @@ void Encoder::UpdateValues() {
   this->ResetCount();
 }
 
-//float Encoder::GetMeters() {
-//  float meters = ((float) this->ticks_ * this->wheel_circumference_)
-//      / TICKS_PER_REVOLUTION;
-//  return meters;
-//}
-
 float Encoder::GetMeters() {
-  float meters = (((float) this->ticks_) / this->ticks_per_meter_);
+  float meters = ((float) this->ticks_ * this->wheel_circumference_)
+      / TICKS_PER_REVOLUTION;
   return meters;
 }
 
-
 float Encoder::GetLinearVelocity() {
   this->UpdateValues();
   float meters = this->GetMeters();

otto_controller/Core/Src/main.cpp

@@ -58,8 +58,8 @@ UART_HandleTypeDef huart6;
 /* USER CODE BEGIN PV */
 
 //Odometry
-Encoder right_encoder = Encoder(&htim5, RIGHT_TICKS_PER_METER);
-Encoder left_encoder = Encoder(&htim2, LEFT_TICKS_PER_METER);
+Encoder right_encoder = Encoder(&htim5, RIGHT_WHEEL_CIRCUMFERENCE);
+Encoder left_encoder = Encoder(&htim2, LEFT_WHEEL_CIRCUMFERENCE);
 Odometry odom = Odometry();
 
 float left_velocity;
@@ -75,25 +75,25 @@ int left_dutycycle;
 int right_dutycycle;
 
 //MotorController
-MotorController left_motor(sleep1_GPIO_Port,
+MotorController right_motor(sleep1_GPIO_Port,
 sleep1_Pin,
-                           dir1_GPIO_Port,
-                           dir1_Pin,
-                           &htim4,
-                           TIM_CHANNEL_4);
-MotorController right_motor(sleep2_GPIO_Port,
-sleep2_Pin,
-                            dir2_GPIO_Port,
-                            dir2_Pin,
+                            dir1_GPIO_Port,
+                            dir1_Pin,
                             &htim4,
-                            TIM_CHANNEL_3);
+                            TIM_CHANNEL_4);
+MotorController left_motor(sleep2_GPIO_Port,
+sleep2_Pin,
+                           dir2_GPIO_Port,
+                           dir2_Pin,
+                           &htim4,
+                           TIM_CHANNEL_3);
 
 //Communication
 uint8_t *tx_buffer;
 uint8_t *rx_buffer;
 
-odometry_msg odom_msg;
 velocity_msg vel_msg;
+wheel_msg wheels_msg;
 
 uint8_t mode = 0;  //setup mode
 
@@ -160,18 +160,16 @@ int main(void) {
 
   left_motor.setup();
   right_motor.setup();
+
   left_motor.coast();
   right_motor.coast();
 
-  tx_buffer = (uint8_t*) &odom_msg;
+  tx_buffer = (uint8_t*) &wheels_msg;
   rx_buffer = (uint8_t*) &vel_msg;
 
   //Enables UART RX interrupt
   HAL_UART_Receive_IT(&huart6, rx_buffer, 8);
 
-  //Enables TIM6 interrupt (used for periodic transmission)
-  HAL_TIM_Base_Start_IT(&htim6);
-
   /* USER CODE END 2 */
 
   /* Infinite loop */
@@ -193,11 +191,11 @@ void SystemClock_Config(void) {
   RCC_ClkInitTypeDef RCC_ClkInitStruct = { 0 };
   RCC_PeriphCLKInitTypeDef PeriphClkInitStruct = { 0 };
 
-  /** Configure the main internal regulator output voltage
+  /** Configure the main internal regulator output voltage 
    */
   __HAL_RCC_PWR_CLK_ENABLE();
   __HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE3);
-  /** Initializes the CPU, AHB and APB busses clocks
+  /** Initializes the CPU, AHB and APB busses clocks 
    */
   RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSI;
   RCC_OscInitStruct.HSIState = RCC_HSI_ON;
@@ -206,7 +204,7 @@ void SystemClock_Config(void) {
   if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK) {
     Error_Handler();
   }
-  /** Initializes the CPU, AHB and APB busses clocks
+  /** Initializes the CPU, AHB and APB busses clocks 
    */
   RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK | RCC_CLOCKTYPE_SYSCLK
       | RCC_CLOCKTYPE_PCLK1 | RCC_CLOCKTYPE_PCLK2;
@@ -237,7 +235,7 @@ static void MX_NVIC_Init(void) {
   HAL_NVIC_SetPriority(TIM6_DAC_IRQn, 2, 2);
   HAL_NVIC_EnableIRQ(TIM6_DAC_IRQn);
   /* USART6_IRQn interrupt configuration */
-  HAL_NVIC_SetPriority(USART6_IRQn, 2, 0);
+  HAL_NVIC_SetPriority(USART6_IRQn, 1, 0);
   HAL_NVIC_EnableIRQ(USART6_IRQn);
 }
 
@@ -481,9 +479,9 @@ static void MX_USART6_UART_Init(void) {
   /* USER CODE END USART6_Init 1 */
   huart6.Instance = USART6;
   huart6.Init.BaudRate = 115200;
-  huart6.Init.WordLength = UART_WORDLENGTH_9B;
+  huart6.Init.WordLength = UART_WORDLENGTH_8B;
   huart6.Init.StopBits = UART_STOPBITS_1;
-  huart6.Init.Parity = UART_PARITY_ODD;
+  huart6.Init.Parity = UART_PARITY_NONE;
   huart6.Init.Mode = UART_MODE_TX_RX;
   huart6.Init.HwFlowCtl = UART_HWCONTROL_NONE;
   huart6.Init.OverSampling = UART_OVERSAMPLING_16;
@@ -591,14 +589,16 @@ void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) {
     left_dutycycle += cross_dutycycle;
     right_dutycycle -= cross_dutycycle;
 
+    wheels_msg.left_vel = left_velocity;
+    wheels_msg.right_vel = right_velocity;
+
   }
 
   //TIMER 2Hz Transmit
   if (htim->Instance == TIM6) {
 
     //TODO odometry
-
-    HAL_UART_Transmit(&huart6, tx_buffer, 8, 100);
+    HAL_UART_Transmit_IT(&huart6, tx_buffer, 8);
   }
 }
 
@@ -625,6 +625,8 @@ void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) {
       mode = 1;
       //Enables TIM3 interrupt (used for PID control)
       HAL_TIM_Base_Start_IT(&htim3);
+      //Enables TIM6 interrupt (used for periodic transmission)
+      HAL_TIM_Base_Start_IT(&htim6);
 
     }
 

otto_controller/otto_controller.ioc

@@ -57,7 +57,7 @@ NVIC.SVCall_IRQn=true\:0\:0\:false\:false\:true\:false\:false
 NVIC.SysTick_IRQn=true\:0\:0\:false\:false\:true\:false\:true
 NVIC.TIM3_IRQn=true\:2\:1\:true\:true\:true\:1\:true\:true
 NVIC.TIM6_DAC_IRQn=true\:2\:2\:true\:true\:true\:2\:true\:true
-NVIC.USART6_IRQn=true\:2\:0\:true\:true\:true\:3\:true\:true
+NVIC.USART6_IRQn=true\:1\:0\:true\:true\:true\:3\:true\:true
 NVIC.UsageFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false
 PA0/WKUP.GPIOParameters=GPIO_Label
 PA0/WKUP.GPIO_Label=encoder_dx1
@@ -230,9 +230,9 @@ TIM6.IPParameters=Prescaler,Period
 TIM6.Period=799
 TIM6.Prescaler=9999
 USART6.IPParameters=VirtualMode-Asynchronous,WordLength,Parity
-USART6.Parity=PARITY_ODD
+USART6.Parity=PARITY_NONE
 USART6.VirtualMode-Asynchronous=VM_ASYNC
-USART6.WordLength=WORDLENGTH_9B
+USART6.WordLength=WORDLENGTH_8B
 VP_SYS_VS_Systick.Mode=SysTick
 VP_SYS_VS_Systick.Signal=SYS_VS_Systick
 VP_TIM3_VS_ClockSourceINT.Mode=Internal

utils/.project

@@ -0,0 +1,11 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<projectDescription>
+	<name>utils</name>
+	<comment></comment>
+	<projects>
+	</projects>
+	<buildSpec>
+	</buildSpec>
+	<natures>
+	</natures>
+</projectDescription>

utils/README.md

@@ -0,0 +1,6 @@
+## Utils
+
+* catikin_ws contains useful ROS nodes.
+* pid_tuning contains Otto code and python scripts for PID tuning
+* py_serial_examples contains pyserial_examples
+* ticks_calibration contains Otto code and python scripts for ticks calibration

utils/catkin_ws/src/joypad_bridge/scripts/cmd_vel_transmitter.py

@@ -6,7 +6,7 @@
 
 ser = serial.Serial(
         baudrate=115200,
-        parity=serial.PARITY_NONE,
+        parity=serial.PARITY_ODD,
         stopbits=serial.STOPBITS_ONE,
         bytesize=serial.EIGHTBITS,
         rtscts=False)