File content as of revision 3:1257a7d2eb3a:

#include "mbed.h"
#include "MODSERIAL.h" //for IMU (and got rid of regular serial library) #include "IMU_code.h" 
#include "StaticDefs.hpp"
#include "ltc1298.hpp"

#include <cstdlib>

#include <string>
using namespace std;

#include "IMU_code.h"                   //IMU code

#include "Battery_Linear_Actuator.h"    //Battery Linear Actuator code (TROY)         (FIX INCLUSION ISSUES, ports)
 
Serial PC(USBTX,USBRX);     //tx, rx

extern "C" void mbed_reset();   //utilized to reset the mbed through the serial terminal

char Key;

string IMU_STRING = "";


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

AnalogIn   pressure_analog_in(A5); //Initialize pin20   (read is float value)
AnalogIn   ain(p18);

/* ************ These tickers work independent of any while loops ********** */
Ticker IMU_ticker; //ticker for printing IMU //https://developer.mbed.org/handbook/Ticker
Ticker BE_position_ticker;  //probably delete soon
Ticker PRESSURE_ticker;

Ticker BCE_ticker;  //new 6/5/17
Ticker PID_ticker;  //new 6/14/17
Ticker LA_ticker;   //new 6/22/17

float positionCmd = 250.0;
/* ************************************************************************* */ 

/* PID LOOP STUFF */
float la_setPoint = 0.00;       //the IMU pitch angle we want (setpoint)

float la_P_gain = 1.0;
float la_I_gain = 0.00;
float la_D_gain = 0.00;
/* PID LOOP STUFF */

float IMU_pitch_angle = 0.00;

bool motor_retracting = false;
bool motor_extending = false;

void IMU_ticking()
{
    led1 = !led1;   //flash the IMU LED

    PC.printf("%s\n", IMU_STRING.c_str());  //if there's something there, print it
}

void PRESSURE_ticking()
{
    PC.printf("pressure: %f mm \r", (0.00122*(adc().ch1_filt)*14.931)-0.0845);    //read the analog pin
    //this voltage has been checked and scaled properly (6/28/2017)
}

void BCE_ticking()  //new 6/5/17
{
    PC.printf("BE_pos: %3.0f mm  BE_vel: %2.2f mm/s  Set Point %3.0f  controller output: % 1.3f \n", pvf().getPosition(), pvf().getVelocity(), positionCmd, posCon().getOutput());
}

int main()
{  
    PC.baud(9600);  //mbed to PC serial connection speed
    //PC.baud(230400);
    //got screwy when i changed it
    hBridge().stop();
    
    PC.printf("Linear Actuator PID Program Started 6/22/17\n");
    
    systemTime().start();   //start the timer, needed for PID loop
        
    /* **************** Linear Actuator MOTOR CONTROLLER **************** */
    Battery_Linear_Actuator BLA_object;             //create the IMU object from the imported class  
    
    PC.printf("%s\n", BLA_object.Keyboard_U().c_str());     //velocity = 0, motor disabled
    
    PC.printf("%s\n", BLA_object.Keyboard_Q().c_str());     //turn off motor
    wait(1);
    PC.printf("%s\n", BLA_object.Keyboard_E().c_str());     //turn on motor
    wait(1);
    
    //setup and start the adc. This runs on a fixed interval and is interrupt driven
    adc().initialize();
    adc().start();

    //Initialize the position velocity filter. This will consume a couple of seconds for
    //the filter to converge
    pvf().init();

////CHANGED TO GLOBAL VARIABLES
    float motor_cmd = 0.0;
//   float positionCmd = 250.0;
    float P = 0.10;
    float I = 0.00;
    float D = 0.00;
    float count = 0.0;
    //char userInput;   //from Trent's code?

    int la_step = 1;
    int la_setPoint_temp = 0;

    hBridge().run(motor_cmd);
    
    //set the intial gains for the position controller
    posCon().setPgain(P);
    posCon().setIgain(I);
    posCon().setDgain(D);
    //posCon().writeSetPoint(positionCmd);
    
    /* *************************** LED *************************** */
    led1 = 1;   //initial values
    led2 = 1;
    led3 = 1;
    led4 = 1;    
    /* *************************** LED *************************** */
    
    PC.printf("Program Started 6/5/17\n");
    //PC.printf("Hit shift + \"H\" to home the battery Linear Actuator\n");
    
    /* *************************** Potentiometer *************************** */
    //PRESSURE_ticker.attach(&PRESSURE_ticking, 1.0);
    /* *************************** Potentiometer *************************** */
    
    /* *************************** MOTOR CONTROLLER *************************** */
    //Battery_Linear_Actuator BLA_object;               //create the IMU object from the imported class
    /* *************************** MOTOR CONTROLLER *************************** */
    
    /* *************************** IMU *************************** */
    IMU_code IMU_object;               //create the IMU object from the imported class
    //IMU_ticker.attach(&IMU_ticking, 5.0);
    /* *************************** IMU *************************** */
    
    /* *************************** BCE *************************** */
    //float previous_positionCmd = -1;
    //BCE_ticker.attach(&BCE_ticking, 5.0);
    /* *************************** BCE *************************** */
    
    while(1) 
    {
        /* *************************** IMU *************************** */
        IMU_STRING = IMU_object.IMU_run(); //grab the IMU string each iteration through the loop
        IMU_pitch_angle = 1.0 * IMU_object.IMU_pitch(); //get the pitch update constantly?
        //PC.printf("pitch angle... %f    set pitch angle: %f\n", IMU_pitch_angle, la_setPoint); 
        /* *************************** IMU *************************** */
        
        /*          Buoyancy Engine         */
        // update the position velocity filter
        pvf().update();

        //update the controller with the current numbers in the position guesser
        posCon().update(pvf().getPosition(), pvf().getVelocity(), pvf().getDt()) ;
        hBridge().run(posCon().getOutput());

        /*          Buoyancy Engine         */
        
        //FOR DEBUGGING
        //PC.printf("BE_pos: %3.0f mm  BE_vel: %2.2f mm/s  Set Point %3.0f  controller output: % 1.3f P: %1.3f I: %1.4f D: %1.4f\r", pvf().getPosition(), pvf().getVelocity(), positionCmd, posCon().getOutput(), P, I, D);
    
        //PC.printf("WHILE LOOP\n");  //DEBUG
        if (PC.readable())
        {
            //led4 != led4;
            //PC.printf("DEBUG: PC IS READABLE\n");  //DEBUG
            
            Key=PC.getc(); 
            if(Key=='w' or Key =='W')
            {
                PC.printf("\nW received!\n");
                
                posCon().writeSetPoint(positionCmd);
                //posCon().setPgain(P);
                //posCon().setIgain(I);
                //posCon().setDgain(D);
                hBridge().run(posCon().getOutput());
                
                hBridge().reset();
                
                count = 0;

            }
            
            /*else if (Key == 'k' or Key=='K')
                PC.printf("%s\n", BLA_object.Keyboard_K().c_str());
            else if (Key == 'l' or Key == 'L')
                PC.printf("%s\n", BLA_object.Keyboard_L().c_str());*/
              
            /*else if(Key =='Q' or Key == 'q')  //motor disabled
            {
                PC.printf("%s\n", BLA_object.Keyboard_Q());
            } 
            
            else if(Key =='E' or Key == 'e')  //motor enabled
            {
                PC.printf("%s\n", BLA_object.Keyboard_E());
            }*/
            
            else if(Key=='p' or Key == 'P')
            {
                PC.printf("### position is %s ###\n", BLA_object.get_pos().c_str());     //flip this back and forth
                wait(10); //for debugging      
                // "-999999" means it is not working 
            }
            else if (Key == 'A' or Key == 'a')
            {
                PC.printf("A recieved\n");
                la_setPoint=la_setPoint_temp;
                PC.printf("LA angle now set to %f\n", la_setPoint);
             }   
            else if (Key=='-' or Key == '_')
            {
                la_setPoint_temp -= la_step; //IMU_pitch_angle -= 1.0;
                PC.printf("- recieved\n");
                PC.printf("LA angle changed to: %d  la_step: %d\n", la_setPoint_temp, la_step); 
            }
            else if (Key =='=' or Key == '+')
            { 
                la_setPoint_temp += la_step; //IMU_pitch_angle += 1.0; 
                PC.printf("+ recieved\n");
                PC.printf("LA angle changed to: %d\n", la_setPoint_temp); 
            }
            else if (Key == '0' or Key == ')')
            {
                PC.printf(") recieved\n");
                if (la_step == 1)
                {
                    la_step = 5;
                }
                else if (la_step == 5)
                {
                    la_step = 10;
                }
                else if (la_step == 10)
                {
                    la_step = 15;
                }
                else if (la_step == 15)
                {
                    la_step = 1;
                }
                PC.printf("LA Step Size Now %d\n", la_step);
            }
            else if (Key=='[' or Key == '{')
            {
                la_P_gain -= 0.1; 
                PC.printf("[ key pressed\n");
                PC.printf("P gain is now %f\n", la_P_gain);
                
            }
            else if (Key==']' or Key == '}')
            {
                la_P_gain += 0.1;
                PC.printf("] key pressed\n");
                PC.printf("P gain is now %f\n", la_P_gain);
                
            }
            else if (Key==';')
            {
                la_I_gain -= 0.1;
                PC.printf("[ key pressed\n");
                PC.printf("P gain is now %f\n", la_P_gain);
                
            
            }
            else if (Key=='\'')
                la_I_gain += 0.1;
            else if (Key=='.')
                la_D_gain -= 0.1;
            else if (Key=='/')
                la_D_gain += 0.1; 
                
            else if(Key=='!')  //RESET THE MBED
            {
                PC.printf("MBED RESET KEY (!) PRESSED\n");
                PC.printf("Linear Actuator Motor disabled! Please re-enable.\n");
                //disable the motor
                BLA_object.Keyboard_Q();        //DISABLE THE MOTOR
                wait(0.5);                      //500 milliseconds
                mbed_reset(); //reset the mbed!
            }
            
            else if(Key =='H')  //homing sequence
            {
                PC.printf("### homing the device ###");
                BLA_object.Keyboard_H();
                wait(10); //for debugging
                
                PC.printf("### position is %d ###\n", BLA_object.get_pos().c_str());     //flip this back and forth
                wait(10); //for debugging       
            }
                //check command against desired control buttons
            else if (Key == 'z' or Key =='Z') {
                //increment the duty cycle
                positionCmd += 10.0 ;
                PC.printf("The position for the buoyancy motor is\nBEP: %3.0f\n", positionCmd); //to read in MATLAB
            } 
            else if (Key == 'x' or Key == 'X') {
                //decrement the duty cycle
                positionCmd -= 10.0 ;
                PC.printf("The position for the buoyancy motor is\nBEP: %3.0f\n", positionCmd); //to read in MATLAB
            }

            else 
            {
                PC.printf("\n%c received!\n", Key);
                PC.printf("\nDoing nothing.\n");
            }
            
            wait_us(100); //for PC readable
            //PC.printf("%s\n", BLA_object.PID_velocity_control(la_setPoint, IMU_pitch_angle, la_P_gain, la_I_gain, la_D_gain).c_str());  //get output string
            //BLA_object.PID_velocity_control(la_setPoint, IMU_pitch_angle, la_P_gain, la_I_gain, la_D_gain).c_str();
        }            
        
        //PC.printf("%s\n", BLA_object.PID_velocity_control(la_setPoint, IMU_pitch_angle, la_P_gain, la_I_gain, la_D_gain).c_str());  //get output string
        BLA_object.PID_velocity_control(la_setPoint, IMU_pitch_angle, la_P_gain, la_I_gain, la_D_gain).c_str();
        wait_us(100); //for PC readable (0.1 ms)
                    
        /*if(BLA_object.MC_readable())   //if you can read the motor controller do this...
        {
            //PC.printf("BATTERY LINEAR ACTUATOR");
            
            
            //PC.printf("Motor Controller response:\n");
//            while(MC.readable())
//            {
//                PC.putc(MC.getc()); //this is a pass-through of the MC (getc) to the PC (putc)
//                wait_ms(1); //1000, 10, 20, 100 (needed at least 1 ms, verified through testing)
//            }
        }
        if ((abs(pvf().getVelocity())<0.1) && (posCon().getOutput()>0.0))
        {
            count ++;
            //pc().printf("We have a small issue\n");
                if(count==10)
                {
                    pc().printf("Bad pot issue\n");
                    hBridge().stop();
                }
            
        }
        else if ((5.0*ain.read())<1.0)
        {
            pc().printf("Hit the limit switch??\n");
            hBridge().stop();
        }
    
        //string snaps
        else if (pvf().getVelocity() > 100)
        {
            //hBridge().stop();
            pc().printf("********** String broke? *********\n");   
        }*/
                
    }
}