added the rest of Giuseppe Caliaro code

diff --git a/pioneer_controller/Core/Inc/firmware/fmw_core.h b/pioneer_controller/Core/Inc/firmware/fmw_core.h
index ab8ff25038b7665bd8cecf19fb0b6862b3c4ce7c..bb20fb060e6d359c98d8db8720bd8cd890ce7735 100644 (file)
--- a/pioneer_controller/Core/Inc/firmware/fmw_core.h
+++ b/pioneer_controller/Core/Inc/firmware/fmw_core.h
@@ -52,7 +52,8 @@ enum {
 
 typedef struct {
   TIM_HandleTypeDef *const timer;
-  uint32_t channel;
+  ADC_HandleTypeDef *const adc;
+  uint32_t timer_channel;
   float voltage_red;
   float voltage_orange;
   float voltage_hysteresis;
@@ -74,9 +75,31 @@ int32_t fmw_encoder_count_get(const FMW_Encoder *encoder);
 int32_t fmw_pid_update(FMW_PidController *pid, float measure);
 
 void fmw_led_init(FMW_Led *led);
-void fmw_led_update(FMW_Led *led, float vin);
+void fmw_led_update(FMW_Led *led);
 
 void fmw_report_handler(FMW_Error error_code, const char *filename, int32_t filename_length, int32_t line);
 void fmw_message_handle(FMW_State state, UART_HandleTypeDef *huart, int32_t wait_ms);
 
+#define FMW_LED_UPDATE_PERIOD 200
+#define FMW_DEBOUNCE_DELAY 200
+
+#define FMW_V_REF 3.3f
+#define FMW_ADC_RESOLUTION 4095.0f
+#define FMW_VIN_SCALE_FACTOR 3.733f
+
+#define FMW_METERS_FROM_TICKS(Ticks, WheelCircumference, TicksPerRevolution) \
+  ((Ticks * WheelCircumference) / TicksPerRevolution)
+
+#define FMW_REPORT_UNLESS(Cond, MessageStatusCode)        \
+  do {                                                    \
+    if (!(Cond)) {                                        \
+      fmw_report_handler(MessageStatusCode, __FILE__,     \
+                          ARRLENGTH(__FILE__), __LINE__); \
+    }                                                     \
+  } while (0)
+
+#define FMW_REPORT(MessageStatusCode)                \
+  fmw_report_handler(MessageStatusCode, __FILE__,    \
+                      ARRLENGTH(__FILE__), __LINE__)
+
 #endif

diff --git a/pioneer_controller/Core/Inc/firmware/fmw_messages.h b/pioneer_controller/Core/Inc/firmware/fmw_messages.h
index 96d4f93e89c6fccd421eda7ba42a395d569bf67f..4f971cd02ec28f6dd83cd31545f5c2cf5da0383e 100644 (file)
--- a/pioneer_controller/Core/Inc/firmware/fmw_messages.h
+++ b/pioneer_controller/Core/Inc/firmware/fmw_messages.h
@@ -83,6 +83,7 @@ typedef struct {
       float voltage_red;
       float voltage_orange;
       float voltage_hysteresis;
+      uint32_t update_period;
     } config_led;
 
     struct {

diff --git a/pioneer_controller/Core/Inc/main.h b/pioneer_controller/Core/Inc/main.h
index 339cbd10e3e5486e0ec17c72579a417f3cac0fef..4dba0dc01940c31a7501ae79d0b99017afa8fc2a 100644 (file)
--- a/pioneer_controller/Core/Inc/main.h
+++ b/pioneer_controller/Core/Inc/main.h
@@ -62,21 +62,6 @@ extern int32_t pid_min;
 \r
 #define ARRLENGTH(Arr) (sizeof((Arr)) / sizeof(*(Arr)))\r
 \r
-#define METERS_FROM_TICKS(Ticks, WheelCircumference, TicksPerRevolution) \\r
-  ((Ticks * WheelCircumference) / TicksPerRevolution)\r
-\r
-#define fmw_report_unless(Cond, MessageStatusCode)        \\r
-  do {                                                    \\r
-    if (!(Cond)) {                                        \\r
-      fmw_report_handler(MessageStatusCode, __FILE__,     \\r
-                          ARRLENGTH(__FILE__), __LINE__); \\r
-    }                                                     \\r
-  } while (0)\r
-\r
-#define fmw_report(MessageStatusCode)                \\r
-  fmw_report_handler(MessageStatusCode, __FILE__,    \\r
-                      ARRLENGTH(__FILE__), __LINE__)\r
-\r
 /* USER CODE END EM */\r
 \r
 void HAL_TIM_MspPostInit(TIM_HandleTypeDef *htim);\r

diff --git a/pioneer_controller/Core/Src/firmware/fwm_core.c b/pioneer_controller/Core/Src/firmware/fwm_core.c
index 20b889f9cef499c738113239dcb325f4c9d01b1e..1c8a0b0a1c557d82e0c0f5949f315fc12026ca83 100644 (file)
--- a/pioneer_controller/Core/Src/firmware/fwm_core.c
+++ b/pioneer_controller/Core/Src/firmware/fwm_core.c
@@ -56,9 +56,9 @@ void fmw_encoder_update(FMW_Encoder *encoder) {
 }
 
 float fmw_encoder_get_linear_velocity(const FMW_Encoder *encoder) {
-  float meters = METERS_FROM_TICKS(encoder->ticks,
-                                   encoder->wheel_circumference,
-                                   encoder->ticks_per_revolution);
+  float meters = FMW_METERS_FROM_TICKS(encoder->ticks,
+                                       encoder->wheel_circumference,
+                                       encoder->ticks_per_revolution);
   float deltatime = encoder->current_millis - encoder->previous_millis;
   float linear_velocity = deltatime > 0.f ? (meters / (deltatime / 1000.f)) : 0.f;
   return linear_velocity;
@@ -102,11 +102,17 @@ int32_t fmw_pid_update(FMW_PidController *pid, float measure) {
 // ============================================================
 // LEDs
 void fmw_led_init(FMW_Led *led) {
-  HAL_TIM_PWM_Start(led->timer, led->channel);
-  __HAL_TIM_SET_COMPARE(led->timer, led->channel, 0);
+  HAL_TIM_PWM_Start(led->timer, led->timer_channel);
+  __HAL_TIM_SET_COMPARE(led->timer, led->timer_channel, 0);
 }
 
-void fmw_led_update(FMW_Led *led, float vin) {
+void fmw_led_update(FMW_Led *led) {
+  HAL_ADC_Start(led->adc);
+  HAL_ADC_PollForConversion(led->adc, HAL_MAX_DELAY);
+  uint32_t adc_val = HAL_ADC_GetValue(led->adc);
+  float v_adc = ((float)adc_val / FMW_ADC_RESOLUTION) * FMW_V_REF;
+  float vin = v_adc * FMW_VIN_SCALE_FACTOR;
+
   uint32_t duty = 0;
   uint32_t arr = led->timer->Instance->ARR;
 
@@ -147,5 +153,5 @@ void fmw_led_update(FMW_Led *led, float vin) {
   } break;
   }
 
-  __HAL_TIM_SET_COMPARE(led->timer, led->channel, duty);
+  __HAL_TIM_SET_COMPARE(led->timer, led->timer_channel, duty);
 }

diff --git a/pioneer_controller/Core/Src/main.c b/pioneer_controller/Core/Src/main.c
index 3dfe9372b44ddb671d3b795a0b23a5c130191283..2eb5b46b32e2aeeacc9126f8ce9e8268e328cbb4 100644 (file)
--- a/pioneer_controller/Core/Src/main.c
+++ b/pioneer_controller/Core/Src/main.c
@@ -23,6 +23,7 @@
 /* Private includes ----------------------------------------------------------*/\r
 /* USER CODE BEGIN Includes */\r
 \r
+#include <math.h>   // M_PI\r
 #include <string.h> // memcpy\r
 \r
 #include "firmware/fmw_inc.h"\r
@@ -68,13 +69,13 @@ static union {
 } encoders = {\r
   .right = {\r
     .timer = &htim3,\r
-    .ticks_per_revolution = 200,\r
-    .wheel_circumference = 0.1f,\r
+    .ticks_per_revolution = 19150,\r
+    .wheel_circumference = (200.f * M_PI) / 1000.f,\r
   },\r
   .left = {\r
     .timer = &htim2,\r
-    .ticks_per_revolution = 200,\r
-    .wheel_circumference = 0.1f,\r
+    .ticks_per_revolution = 19150,\r
+    .wheel_circumference = (200.f * M_PI) / 1000.f,\r
   },\r
 };\r
 \r
@@ -135,14 +136,30 @@ static union {
   },\r
 };\r
 \r
+FMW_Led pled = {\r
+  .adc = &hadc1,\r
+  .timer = &htim1,\r
+  .timer_channel = TIM_CHANNEL_1,\r
+  .voltage_red = 11.5f,\r
+  .voltage_orange = 12.5f,\r
+  .voltage_hysteresis = 0.05f,\r
+  .state = FMW_LedState_Red,\r
+};\r
+\r
 int32_t pid_max = 0;\r
 int32_t pid_min = 0;\r
 \r
+static uint32_t led_update_period = 200;\r
+\r
 static volatile int32_t ticks_left  = 0;\r
 static volatile int32_t ticks_right = 0;\r
 static volatile float previous_tx_millis;\r
 static volatile uint8_t tx_done_flag = 1;\r
 /* volatile MessageStatusCode otto_status = MessageStatusCode_Waiting4Config; */\r
+\r
+static volatile uint32_t time_aux_press = 0;\r
+static volatile uint32_t time_aux2_press = 0;\r
+static volatile uint32_t time_last_motors = 0;\r
 static volatile FMW_State fmw_state = FMW_State_Init;\r
 \r
 /* USER CODE END PV */\r
@@ -765,6 +782,7 @@ __attribute__((noreturn)) void start(void) {
 \r
   fmw_encoder_init(encoders.values);\r
   fmw_motor_init(motors.values);\r
+  fmw_led_init(&pled);\r
 \r
   //right and left motors have the same parameters\r
   pid_max = (int32_t)htim4.Instance->ARR;\r
@@ -778,14 +796,20 @@ __attribute__((noreturn)) void start(void) {
   HAL_UART_Receive_DMA(&huart6, (uint8_t*) &vel_msg, 12);\r
 #endif\r
 \r
-  while (1);\r
+  for (uint32_t time_last_led_update = 0;;) {\r
+    uint32_t time_now = HAL_GetTick();\r
+    if (time_now - time_last_led_update >= led_update_period) {\r
+      time_last_led_update = time_now;\r
+      fmw_led_update(&pled);\r
+    }\r
+  }\r
 }\r
 \r
 void fmw_message_handle(FMW_State state, UART_HandleTypeDef *huart, int32_t wait_ms) {\r
   FMW_Message msg = {0};\r
   HAL_StatusTypeDef uart_packet_status = HAL_UART_Receive(huart, (uint8_t*)&msg, sizeof(msg), wait_ms);\r
-  fmw_report_unless(uart_packet_status == HAL_OK, FMW_Error_UART_ReceiveTimeoutElapsed);\r
-  fmw_report_unless(msg.header.type < FMW_MessageType_COUNT, FMW_Error_Command_NotRecognized);\r
+  FMW_REPORT_UNLESS(uart_packet_status == HAL_OK, FMW_Error_UART_ReceiveTimeoutElapsed);\r
+  FMW_REPORT_UNLESS(msg.header.type < FMW_MessageType_COUNT, FMW_Error_Command_NotRecognized);\r
 \r
   uint32_t crc_received = msg.header.crc;\r
   msg.header.crc = 0;\r
@@ -794,12 +818,12 @@ void fmw_message_handle(FMW_State state, UART_HandleTypeDef *huart, int32_t wait
     switch (msg.header.type) {\r
     case FMW_MessageType_Run: {\r
       uint32_t crc_computed = HAL_CRC_Calculate(&hcrc, (uint32_t*)&msg, sizeof msg.header);\r
-      fmw_report_unless(crc_received == -1 || crc_computed == crc_received, FMW_Error_UART_Crc);\r
+      FMW_REPORT_UNLESS(crc_received == -1 || crc_computed == crc_received, FMW_Error_UART_Crc);\r
       fmw_state = FMW_State_Running;\r
     } break;\r
     case FMW_MessageType_Config_Robot: {\r
       uint32_t crc_computed = HAL_CRC_Calculate(&hcrc, (uint32_t*)&msg, sizeof msg.header.type + sizeof msg.config_robot);\r
-      fmw_report_unless(crc_received == -1 || crc_computed == crc_received, FMW_Error_UART_Crc);\r
+      FMW_REPORT_UNLESS(crc_received == -1 || crc_computed == crc_received, FMW_Error_UART_Crc);\r
 \r
       odometry.baseline = msg.config_robot.baseline;\r
       encoders.left.wheel_circumference   = msg.config_robot.left_wheel_circumference;\r
@@ -809,12 +833,21 @@ void fmw_message_handle(FMW_State state, UART_HandleTypeDef *huart, int32_t wait
     } break;\r
     case FMW_MessageType_Config_PID: {\r
       uint32_t crc_computed = HAL_CRC_Calculate(&hcrc, (uint32_t*)&msg, sizeof msg.header.type + sizeof msg.config_pid);\r
-      fmw_report_unless(crc_received == -1 || crc_computed == crc_received, FMW_Error_UART_Crc);\r
+      FMW_REPORT_UNLESS(crc_received == -1 || crc_computed == crc_received, FMW_Error_UART_Crc);\r
 \r
       memcpy(&pid_left.ks,  &msg.config_pid.left,  sizeof pid_left.ks);\r
       memcpy(&pid_right.ks, &msg.config_pid.right, sizeof pid_right.ks);\r
       memcpy(&pid_cross.ks, &msg.config_pid.cross, sizeof pid_cross.ks);\r
     } break;\r
+    case FMW_MessageType_Config_LED: {\r
+      uint32_t crc_computed = HAL_CRC_Calculate(&hcrc, (uint32_t*)&msg, sizeof msg.header.type + sizeof msg.config_led);\r
+      FMW_REPORT_UNLESS(crc_received == -1 || crc_computed == crc_received, FMW_Error_UART_Crc);\r
+\r
+      pled.voltage_red = msg.config_led.voltage_red;\r
+      pled.voltage_orange = msg.config_led.voltage_orange;\r
+      pled.voltage_hysteresis = msg.config_led.voltage_hysteresis;\r
+      led_update_period = msg.config_led.update_period;\r
+    } break;\r
     }\r
   } break;\r
   }\r
@@ -829,6 +862,16 @@ void fmw_report_handler(FMW_Error error_code, const char *filename, int32_t file
   for (;;) {}\r
 }\r
 \r
+// TODO(lb): move to fmw. maybe create a FMW_Buzzer?\r
+void buzzer_set(bool on) {\r
+  if (on) {\r
+    __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_2, htim1.Init.Period / 2);\r
+    HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_2);\r
+  } else {\r
+    HAL_TIM_PWM_Stop(&htim1, TIM_CHANNEL_2);\r
+  }\r
+}\r
+\r
 // TIMER 100Hz PID control\r
 void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) {\r
   // NOTE(lb): for transmission\r
@@ -927,13 +970,53 @@ void HAL_UART_ErrorCallback(UART_HandleTypeDef *UartHandle) {
 }\r
 \r
 void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) {\r
-  //Blue user button on the NUCLEO board\r
+  uint32_t time_now = HAL_GetTick();\r
+\r
   switch (GPIO_Pin) {\r
+  case aux_Pin: {\r
+    if (time_now - time_aux_press > FMW_DEBOUNCE_DELAY) {\r
+      time_aux_press = time_now;\r
+      HAL_GPIO_TogglePin(GPIOB, LD1_Pin);\r
+      HAL_GPIO_TogglePin(GPIOB, LD2_Pin);\r
+      HAL_GPIO_TogglePin(GPIOB, LD3_Pin);\r
+      /* char msg[] = "AUX1 button pressed\r\n"; */\r
+      /* HAL_UART_Transmit(&huart3, (uint8_t*)msg, strlen(msg), HAL_MAX_DELAY); */\r
+    }\r
+  } break;\r
+  case aux2_Pin: {\r
+    if (time_now - time_aux2_press > FMW_DEBOUNCE_DELAY) {\r
+      time_aux2_press = time_now;\r
+      /* char msg[] = "AUX2 button pressed\r\n"; */\r
+      /* HAL_UART_Transmit(&huart3, (uint8_t*)msg, strlen(msg), HAL_MAX_DELAY); */\r
+    }\r
+  } break;\r
+  case motors_btn_Pin: {\r
+    if (time_now - time_last_motors > FMW_DEBOUNCE_DELAY) {\r
+      time_last_motors = time_now;\r
+      /* char msg[] = "Motors button pressed\r\n"; */\r
+      /* HAL_UART_Transmit(&huart3, (uint8_t*)msg, strlen(msg), HAL_MAX_DELAY); */\r
+      if(motors.left.active && motors.right.active) {\r
+        fmw_motor_disable(&motors.left);\r
+        fmw_motor_disable(&motors.right);\r
+        HAL_GPIO_WritePin(SLED_GPIO_Port, SLED_Pin, GPIO_PIN_RESET);\r
+        buzzer_set(false);\r
+        /* char msg[] = "Motors OFF\r\n"; */\r
+        /* HAL_UART_Transmit(&huart3, (uint8_t*)msg, strlen(msg), HAL_MAX_DELAY); */\r
+      } else {\r
+        fmw_motor_enable(&motors.left);\r
+        fmw_motor_enable(&motors.right);\r
+        HAL_GPIO_WritePin(SLED_GPIO_Port, SLED_Pin, GPIO_PIN_SET);\r
+        buzzer_set(false);\r
+        /* char msg[] = "Motors ON\r\n"; */\r
+        /* HAL_UART_Transmit(&huart3, (uint8_t*)msg, strlen(msg), HAL_MAX_DELAY); */\r
+      }\r
+    }\r
+  } break;\r
   case fault1_Pin:\r
   case fault2_Pin: {\r
     fmw_motor_brake(&motors.left);\r
     fmw_motor_brake(&motors.right);\r
-    //stop TIM6 interrupt (used for PID control)\r
+    // stop TIM6 interrupt (used for PID control)\r
     HAL_TIM_Base_Stop_IT(&htim6);\r
     /* otto_status = MessageStatusCode_Fault_HBridge; */\r
   } break;\r

diff --git a/pioneer_workstation/src/main.c b/pioneer_workstation/src/main.c
index 1dd6af1406396e7265c29761c37811828ad185e9..c4a96927b2c5f4194749dce2ea6b3812dcc2df2b 100644 (file)
--- a/pioneer_workstation/src/main.c
+++ b/pioneer_workstation/src/main.c
@@ -11,11 +11,30 @@
 
 // TODO(lb): implement CRC
 
+static int serial_open(char *portname) {
+  int fd = open(portname, O_RDWR | O_NOCTTY | O_SYNC);
+  assert(fd >= 0);
+  tcflush(fd, TCIOFLUSH);
+
+  struct termios tty;
+  assert(!tcgetattr(fd, &tty));
+  cfsetospeed(&tty, B115200); // output baud rate
+  cfsetispeed(&tty, B115200); //  input baud rate
+  tty.c_cflag = (tty.c_cflag & (tcflag_t)~CSIZE) | CS8; // 8-bit chars
+  tty.c_iflag &= (tcflag_t)~IGNBRK;                     // disable break processing
+  tty.c_cc[VMIN]  = 0;                                  // read doesn't block
+  tty.c_cc[VTIME] = 20;                                 // 2.0 seconds read timeout
+  tty.c_cflag &= (tcflag_t)~(PARENB | PARODD);          // no parity
+  tty.c_cflag &= (tcflag_t)~CSTOPB;                     // 1 stop bit
+  assert(!tcsetattr(fd, TCSANOW, &tty));
+  return fd;
+}
+
 int main(void) {
-  int fd = open("/dev/ttyACM0", O_RDWR);
+  int fd = serial_open("/dev/ttyACM0");
   assert(fd);
 
-#if 0
+#if 1
   FMW_Message msg_config_robot = {
     .header = {
       .type = FMW_MessageType_Config_Robot,