<commit_before>#include <cstring>

#include "SampleAndControl.h"
#include "chprintf.h"

#include "MPU6050.h"
#include "RearWheel.h"
#include "YawRateController.h"
#include "SampleBuffer.h"
#include "SystemState.h"

SampleAndControl::SampleAndControl()
  : tp_control_(0), state_(0)
{
}

void SampleAndControl::controlThread(const char * filename)
{
  chRegSetThreadName("Control");
  enableSensorsMotors();
  MPU6050 & imu = MPU6050::Instance();
  RearWheel & rwc = RearWheel::Instance();
  YawRateController & yrc = YawRateController::Instance();
  SampleBuffer & sb = SampleBuffer::Instance();
  sb.initialize(filename);
  
  // zero out wheel encoders and system timer
  STM32_TIM4->CNT = STM32_TIM5->CNT = STM32_TIM8->CNT = 0;

  // Create a sample to populate
  Sample s;
  memset(&s, 0, sizeof(s));

  systime_t time = chTimeNow();     // Initial time
  systime_t sleep_time;
  for (uint32_t i = 0; !chThdShouldTerminate(); ++i) {
    time += MS2ST(con::T_ms);       // Next deadline

    // Begin pre control data collection
    sampleTimers(s);  // sample system time/encoder counts/PWM duty cycle
    imu.Acquire(s);   // sample rate gyro, accelerometer and temperature sensors
    sampleSetPoints(s); // sample rear wheel and yaw rate commands
    // End pre control data collection

    // Begin control
    if (rwc.isEnabled() && (i % con::RW_N == 0))
      rwc.Update(s);

    if (yrc.isEnabled() && (i % con::YC_N == 0))
      yrc.Update(s);
    // End control

    // Begin post control data collection
    sampleMotorState(s);
    s.system_state |= systemstate::CollectionEnabled;
    // End post control data collection

    // Put the sample in to the buffer
    bool encode_failure = false;
    if (!sb.insert(s))
      encode_failure = true;

    // Clear the sample for the next iteration
    // The first time through the loop, computation_time will be logged as zero,
    // subsequent times will be accurate but delayed by one sample period
    uint32_t ti = s.system_time;
    memset(&s, 0, sizeof(s));
    s.computation_time = STM32_TIM5->CNT - ti;
    if (encode_failure) 
      s.system_state |= systemstate::SampleBufferEncodeError;

    // Go to sleep until next interval
    chSysLock();<commit_msg>WIP: Replacing old yaw rate and rear motor controllers with new ones.<commit_after>#include <cstring>

#include "SampleAndControl.h"
#include "chprintf.h"

#include "MPU6050.h"
#include "rear_motor_controller.h"
#include "fork_motor_controller.h"
#include "SampleBuffer.h"
#include "SystemState.h"

SampleAndControl::SampleAndControl()
  : tp_control_(0), state_(0)
{
}

void SampleAndControl::controlThread(const char * filename)
{
  chRegSetThreadName("Control");
  enableSensorsMotors();
  MPU6050 & imu = MPU6050::Instance();
  hardware::RearMotorController rear_motor_controller;
  hardware::ForkMotorController fork_motor_controller;
  hardware::Encoder front_wheel_encoder(STM32_TIM4, 800);
  front_wheel_encoder.set_count(0);

  SampleBuffer & sb = SampleBuffer::Instance();
  sb.initialize(filename);
  
  // zero out system timer
  STM32_TIM5->CNT = 0;

  // Create a sample to populate
  Sample s;
  memset(&s, 0, sizeof(s));

  systime_t time = chTimeNow();     // Initial time
  systime_t sleep_time;
  for (uint32_t i = 0; !chThdShouldTerminate(); ++i) {
    time += MS2ST(con::T_ms);       // Next deadline

    // Begin pre control data collection
    sampleTimers(s);  // sample system time/encoder counts/PWM duty cycle
    imu.Acquire(s);   // sample rate gyro, accelerometer and temperature sensors
    sampleSetPoints(s); // sample rear wheel and yaw rate commands
    // End pre control data collection

    // Begin control
    // if (rwc.isEnabled() && (i % con::RW_N == 0))
    rear_motor_controller.update(s);
    // if (yrc.isEnabled() && (i % con::YC_N == 0))
    fork_motor_controller.update(s);
    // End control

    // Begin post control data collection
    sampleMotorState(s);
    s.system_state |= systemstate::CollectionEnabled;
    // End post control data collection

    // Put the sample in to the buffer
    bool encode_failure = false;
    if (!sb.insert(s))
      encode_failure = true;

    // Clear the sample for the next iteration
    // The first time through the loop, computation_time will be logged as zero,
    // subsequent times will be accurate but delayed by one sample period
    uint32_t ti = s.system_time;
    memset(&s, 0, sizeof(s));
    s.computation_time = STM32_TIM5->CNT - ti;
    if (encode_failure) 
      s.system_state |= systemstate::SampleBufferEncodeError;

    // Go to sleep until next interval
    chSysLock();
{'fraction_non_alphanumeric': 0.11652588260775396, 'fraction_numerical': 0.016244314489928524, 'mean_word_length': 3.549753694581281, 'pattern_counts': {'":': 0, '<': 5, '<?xml version=': 0, '>': 11, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '4990584', 'n_tokens_mistral': 1538, 'n_tokens_neox': 1457, 'n_words': 411}