File content as of revision 9:a8fd0bd49279:

//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"

//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

//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;

//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

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"

// ================================================================
// ===                      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;

    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 it 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
            
            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();
            }
        }


        //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;
        /*****************************************/

        //Running: Motor Control Enabled
        if(ROBOT_ON) {
            //Enable Motor
            enable = ENABLE;

            //Calculate motor inputs
            motor1 = int16_t(throttle/2 + steering/8);
            motor2 = int16_t(throttle/2 - steering/8);

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

            //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()