File content as of revision 10:5ef5fe8c7775:

//Balance Bot V4
//Author: Carter Sharer
//Date: 10/13/2016
//ESE519 Lab 6 Part 3 Skeleton Code

/******************************* README USAGE *******************************
* This robot must be powered on while it is laying down flat on a still table
* This allows the robot to calibrate the IMU (~5 seconds)
* The motors are DISABLED when the robot tilts more then +-45 degrees from
* vertical.  To ENABLE the motors you must lift the robot to < +- 45 degres and
* press the joystick button.
* To reset the motor positions you must press the josystick button anytime.
******************************************************************************/

//Balance Bot Begin
#include "mbed.h"
#include "pin_assignments.h"
#include "balance_bot.h"
#include "stepper_motors.h"
#include "MRF24J40.h"

#define WHEEL_BASE 100 //mm
#define NUM_STEPS 1600 //number of steps for 1 turn of wheel
#define WHEEL_RADIUS 51 //units in mm
#define PI 3.1415
#define maxAccel 5
//#define CIRC 2*PI*WHEEL_RADIUS

//For RF Communication
#define JSTICK_H 8
#define JSTICK_V 9
#define SPACE 10
#define KNOB1 11
#define KNOB2 12
#define KNOB3 13
#define KNOB4 14
#define BUTTON 16
#define JSTICK_OFFSET 100
#define TX_BUFFER_LEN 18
#define TX_ANGLE_OFFSET 100
//Knobs
#define POT1 p17
#define POT2 p18
#define POT3 p16
#define POT4 p15
//JoyStick
#define POTV p19
#define POTH p20
#define X 500
#define Y 0


//PID
#define MAX_THROTTLE 100
#define MAX_TARGET_ANGLE 12
//PID Default control values from constant definitions
float Kp1;
float Kd1;
float Kp2;
float Kd2;
int i;

//Controller Values
uint8_t knob1, knob2, knob3, knob4;
int8_t jstick_h, jstick_v;

//Control Variables
float throttle = 0; //From joystick
float steering = 0; //From joystick
int robot_pos = 0; //Robots position
float motor1_old = 0; 

Timer timer;
int timer_value;
int timer_old;
int dt;

//Loop Counters
uint8_t slow_loop_counter;
uint8_t medium_loop_counter;
uint8_t loop_counter;

Serial pc(USBTX, USBRX);

// LEDs
DigitalOut led1(LED1);
DigitalOut led2(LED2);
DigitalOut led3(LED3);
DigitalOut led4(LED4);

//Button
bool button;
#include "communication.h"

//Waypoint
//[(10m, 0th). (5m, 90th)]
//(10x, 0y), (x, y)]

// ================================================================
// ===                      INITIAL SETUP                       ===
// ================================================================

// ================================================================
// ===                    MAIN PROGRAM LOOP                     ===
// ================================================================
int main()
{
    //Used to toggle motors on and off
    bool ROBOT_ON = false;

    //Set the Channel. 0 is default, 15 is max
    uint8_t channel = 9;
    mrf.SetChannel(channel);

    //Used for button press
    int last_button_time = 0;
    float wheel1_dist = 0;
    float wheel2_dist = 0;
    float robot_theta = 0;
    float distance_traveled = 0;
    float world_x = 0;
    float world_y = 0;
    float robot_pos_old = 0;

    pc.baud(115200);
    pc.printf("Start\r\n");
    timer.start();
    //timer
    timer_value = timer.read_us();

    //Init Stepper Motors
    //Attach Timer Interupts (Tiker)
    timer_M1.attach_us(&ISR1, ZERO_SPEED);
    timer_M2.attach_us(&ISR2, ZERO_SPEED);
    step_M1 = 1;
    dir_M1 = 1;
    enable = DISABLE; //Disable Motors

    //Enable Motors
    enable = ENABLE;

    while(1) {
        //Led 4 to indicate if robot is Running or Not
        led4 = ROBOT_ON;

        //Led 2 to indicate a button press
        led2 = button;

        //If button is pressed reset motor position
        if(button) {
            pos_M1 = 0; //Reset position of Motor 1
            pos_M2 = 0; //Reset position of motor 2

            //Read Button Press to Toggle Motors
            if((timer.read_us() - last_button_time) > 250000) {
                ROBOT_ON = ROBOT_ON^1;
                pc.printf("BUTTON WAS PRESSED \r\n");
                last_button_time = timer.read_us();
            }
        }

        //Conver Motor Position from Steps to Distance
        wheel1_dist = 2*PI*WHEEL_RADIUS* pos_M1/NUM_STEPS;
        wheel2_dist = 2*PI*WHEEL_RADIUS* pos_M2/NUM_STEPS;
        robot_pos = (wheel1_dist + wheel2_dist) / 2; ///total dist traveled
        robot_theta = (wheel1_dist - wheel2_dist) /WHEEL_BASE; //angle

        distance_traveled = robot_pos - robot_pos_old;
    
    
        //Curr Robot X and Y
        world_x += distance_traveled * sin(robot_theta); //import sin
        world_y += distance_traveled * cos(robot_theta);
        
        //Pose(x,y,th) = P(world_x, world_y, robot_th)

        pc.printf("X axis: %d",world_x);
        pc.printf("Y axis: %d \n",world_y);
        pc.printf("robot_pos: %d \n",robot_pos);
        //Timer
        timer_value = timer.read_us();

        //Joystick control
        throttle = jstick_v;
        steering = jstick_h;

        /**** Update Values DO NOT MODIFY ********/
        loop_counter++;
        slow_loop_counter++;
        medium_loop_counter++;
        dt = (timer_value - timer_old);
        timer_old = timer_value;
        robot_pos_old = robot_pos;
        /*****************************************/

        //Running: Motor Control Enabled
        if(ROBOT_ON) {
            //TEMPWAYPOINTS
            //Get Waypoint
            //i = 0;
            //X = WAYPOINS[i][0]
            //Y = WAYPOINS[i][1]
            //calc target theta
            float target_theta = 0;
            float target_distance = 5000;
            //if(robot is at waypoint)
            //get new waypoint
            //i++
            //else {
                //}
            
            
            
            //Enable Motor
            enable = ENABLE;

            //User Control ONLY 
            float theta_error = target_theta - robot_theta;

            if(theta_error > 1 && theta_error < -1) {
                //Angular Controller
                float turn_speed = theta_error*0.5;
                motor1 = turn_speed;
                motor2 = -turn_speed; 
            } else {
                //Dist Controller
                float dist_error = target_distance - robot_pos;
                float speed = dist_error*0.5;
                motor1 = speed;
                motor2 = speed;
            }
            
            
            //Acceleratoin Cap
            float motor1Acel = motor1 - motor1_old;
            
            if(motor1Acel > maxAccel) { //macAcel = 5
                motor1 = maxAccel;
            }

            //Cap the max and min values [-100, 100]
            motor1 = CAP(motor1, MAX_CONTROL_OUTPUT);
            motor2 = CAP(motor2, MAX_CONTROL_OUTPUT);
            motor1_old = motor1;

            //Set Motor Speed here
            setMotor1Speed(motor1);
            setMotor2Speed(motor2);

        }
        //Robot is off so disable the motors
        else {
            enable = DISABLE;
        }

        /* Here are some loops that trigger at different intervals, this
        * will allow you to do things at a slower rate, thus saving speed
        * it is important to keep this fast so we dont miss IMU readings */

        //Fast Loop: Good for printing to serial monitor
        if(loop_counter >= 5) {
            loop_counter = 0;
            pc.printf("ROBOT_ON:%d pos_M1:%d pos_M2:%d robot_pos%d \r\n", ROBOT_ON, pos_M1, pos_M2, robot_pos);
        }

        //Meduim Loop: Good for sending and receiving
        if (medium_loop_counter >= 10) {
            medium_loop_counter = 0; // Read  status

            //Recieve Data
            rxLen = rf_receive(rxBuffer, 128);
            if(rxLen > 0) {
                led1 = led1^1;
                //Process data with our protocal
                communication_protocal(rxLen);
            }

        }  // End of medium loop

        //Slow Loop: Good for sending if speed is not an issue
        if(slow_loop_counter >= 99) {
            slow_loop_counter = 0;

            /* Send Data To Controller goes here *
             *                                   */
        } //End of Slow Loop



    } //end main loop
} //End Main()