<commit_before>/* Program for my ballbot, found at http://onewaytobalance.blogspot.com
 * 
 * Program pretty much runs on an interrupt.
 * Configurations, settings and global variables/objects found in ballbot.h
 *
 * Only runs on Teensy 3.x and requires a decent clock (I use 96MHz)
 * This is because it is quite processing intensive since the single MCU does
 *    - on board Mahony sensor fusion at >1000Hz using 9dof data from the MPU9250
 *          --> converting quaternions to Euler angles unfortunately requires CPU intensive 
 *                inverse trigonometry
 *    - PID roll/pitch control in 2 dimensions (i.e. two PID control loops for balancing)
 *    - Polling encoders at a high sample rate (64cpr encoders and a 12400rpm motor - no load)
 *            --> Finding velocity of each of the 3 wheels
 *    - Using wheel velocities and trig to obtain absolate location/position of robot
 *          see http://onewaytobalance.blogspot.com.au/2015/12/omnidirectional-control.html
 *          The maths requires you to have a basic understanding of linear algebra (matrices etc.)
 *    - PID control for position of robot
 *    - Debugging via bluetooth and serial to computer (the RN42 is quite slow)
 *    
 * TODO
 *    - Improve magnetometer calibration routine
 *    - Implemented tilt compensated bearing calculations with magnetometer
 *    - Pass tilt compensated bearing to position calculator
 *    - Implement yaw correction
 *    - 
 *    
 * (C) <NAME> 2015
 */

#include <SPI.h>
#include <EEPROM.h>
#include <FrequencyTimer2.h>   // Built into teensy core

/* own libraries */
#include <EEPROMAnything.h>    // see https://github.com/brianchen118/Team-PI-Lib/tree/master/EEPROMAnything
#include <MPU9250.h>           // see https://github.com/brianchen118/MPU9250
#include <pwmMotor.h>          // see https://github.com/brianchen118/Team-PI-Lib/tree/master/pwmMotor
#include <fastTrig.h>          // https://github.com/brianchen118/fastTrig
#include <omnidrive.h>         // see https://github.com/brianchen118/omnidrive
#include <sensorfusion.h>      // see https://github.com/brianchen118/sensorfusion
#include <PID_v1.h>
#include "balanceController.h"
#include "locationCalculator.h"
#include "ballbot.h"

#define POS_LPF_T 200.0f
elapsedMicros posLPF_dt;

#define BAUD_9600 0
#define BAUD_115200 1
#define BAUD_230400 2
#define BAUD_460800 3
#define BAUD_921600 4
#define BAUD_1382400 5

#define RN42 0
#define HC06 1

void btSetup(uint8_t module, int initBaud, int finalBaud){
    if (module == 0){

    }
    else if (module == 1){
        BT.begin(initBaud);
        delay(100);
        Serial.print("AT");
        BT.println("AT");

        delay(1000);

        switch (finalBaud){
            case 0: BT.print("AT+BAUD4"); delay(500); BT.begin(9600);   break;
            case 1: BT.print("AT+BAUD8"); delay(500); BT.begin(115200); break;
            case 2: BT.print("AT+BAUD9"); delay(500); BT.begin(230400); break;
            case 3: BT.print("AT+BAUDA"); delay(500); BT.begin(460800); break;
            case 4: BT.print("AT+BAUDB"); delay(500); BT.begin(921600); break;
            case 5: BT.print("AT+BAUDC"); delay(500); BT.begin(1382400); break;
        }
    }
}

/* Main part of the program begins here.
 *<commit_msg>Changed 0 and 1 to RN42 and HC06 (defined)<commit_after>/* Program for my ballbot, found at http://onewaytobalance.blogspot.com
 * 
 * Program pretty much runs on an interrupt.
 * Configurations, settings and global variables/objects found in ballbot.h
 *
 * Only runs on Teensy 3.x and requires a decent clock (I use 96MHz)
 * This is because it is quite processing intensive since the single MCU does
 *    - on board Mahony sensor fusion at >1000Hz using 9dof data from the MPU9250
 *          --> converting quaternions to Euler angles unfortunately requires CPU intensive 
 *                inverse trigonometry
 *    - PID roll/pitch control in 2 dimensions (i.e. two PID control loops for balancing)
 *    - Polling encoders at a high sample rate (64cpr encoders and a 12400rpm motor - no load)
 *            --> Finding velocity of each of the 3 wheels
 *    - Using wheel velocities and trig to obtain absolate location/position of robot
 *          see http://onewaytobalance.blogspot.com.au/2015/12/omnidirectional-control.html
 *          The maths requires you to have a basic understanding of linear algebra (matrices etc.)
 *    - PID control for position of robot
 *    - Debugging via bluetooth and serial to computer (the RN42 is quite slow)
 *    
 * TODO
 *    - Improve magnetometer calibration routine
 *    - Implemented tilt compensated bearing calculations with magnetometer
 *    - Pass tilt compensated bearing to position calculator
 *    - Implement yaw correction
 *    - 
 *    
 * (C) <NAME> 2015
 */

#include <SPI.h>
#include <EEPROM.h>
#include <FrequencyTimer2.h>   // Built into teensy core

/* own libraries */
#include <EEPROMAnything.h>    // see https://github.com/brianchen118/Team-PI-Lib/tree/master/EEPROMAnything
#include <MPU9250.h>           // see https://github.com/brianchen118/MPU9250
#include <pwmMotor.h>          // see https://github.com/brianchen118/Team-PI-Lib/tree/master/pwmMotor
#include <fastTrig.h>          // https://github.com/brianchen118/fastTrig
#include <omnidrive.h>         // see https://github.com/brianchen118/omnidrive
#include <sensorfusion.h>      // see https://github.com/brianchen118/sensorfusion
#include <PID_v1.h>
#include "balanceController.h"
#include "locationCalculator.h"
#include "ballbot.h"

#define POS_LPF_T 200.0f
elapsedMicros posLPF_dt;

#define BAUD_9600 0
#define BAUD_115200 1
#define BAUD_230400 2
#define BAUD_460800 3
#define BAUD_921600 4
#define BAUD_1382400 5

#define RN42 0
#define HC06 1

void btSetup(uint8_t module, int initBaud, int finalBaud){
    if (module == RN42){

    }
    else if (module == HC06){
        BT.begin(initBaud);
        delay(100);
        Serial.print("AT");
        BT.println("AT");

        delay(1000);

        switch (finalBaud){
            case 0: BT.print("AT+BAUD4"); delay(500); BT.begin(9600);   break;
            case 1: BT.print("AT+BAUD8"); delay(500); BT.begin(115200); break;
            case 2: BT.print("AT+BAUD9"); delay(500); BT.begin(230400); break;
            case 3: BT.print("AT+BAUDA"); delay(500); BT.begin(460800); break;
            case 4: BT.print("AT+BAUDB"); delay(500); BT.begin(921600); break;
            case 5: BT.print("AT+BAUDC"); delay(500); BT.begin(1382400); break;
        }
    }
}

/* Main part of the program begins here.
 *
{'fraction_non_alphanumeric': 0.11095911232710139, 'fraction_numerical': 0.057151542787657696, 'mean_word_length': 2.9567047504509922, 'pattern_counts': {'":': 0, '<': 25, '<?xml version=': 0, '>': 31, 'https://': 12, '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': '6758522', 'n_tokens_mistral': 2397, 'n_tokens_neox': 2055, 'n_words': 638}