Diff: main.cpp

Revision:

0:381a84fad08b

Child:

2:c3cb3ea3c9fa

diff -r 000000000000 -r 381a84fad08b main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Jun 09 17:32:33 2017 +0000
@@ -0,0 +1,280 @@
+#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 pin_ENA(p23);    //ENA       //DigitalOut PinName (pin) //Troy: IF WE NEED IT
+DigitalOut pin_IN1(p22);    //IN1      (changed from pin11 and 12)   
+DigitalOut pin_IN2(p21);    //IN2       //should I use wait or interrupt to control this?
+
+DigitalOut led1(LED1);
+DigitalOut led2(LED2);
+DigitalOut led3(LED3);
+DigitalOut led4(LED4);
+
+DigitalOut enab(p9);        // a/d converter enable
+
+AnalogIn   pressure_analog_in(A5); //Initialize pin20   (read is float value)
+
+/* ************ 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
+/*float motor_cmd = 0.0;
+float positionCmd = 250.0;
+float P = 0.10;
+float I = 0.00;
+float D = 0.00;
+*/
+float positionCmd = 250.0;
+/* ************************************************************************* */ 
+
+void IMU_ticking()
+{
+    led1 = !led1;   //flash the IMU LED
+    
+    if (!IMU_STRING.empty())
+        PC.printf("%s\n", IMU_STRING);  //if there's something there, print it
+}
+
+void PRESSURE_ticking()
+{
+    PC.printf("Pressure sensor (PSI)\nPRESS_PSI: %3.3f\n", 3.3*1.503*10 * pressure_analog_in.read());    //read the analog pin
+    //this voltage has been checked and scaled properly (5/19/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(115200);  //mbed to PC serial connection speed
+    //PC.baud(230400);
+    //got screwy when i changed it
+    hBridge().stop();
+    //start up the system timer so the position velocity estimator has a time signal
+    systemTime().start();
+    enab=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?
+
+    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, 2.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, 2.0);
+    /* *************************** IMU *************************** */
+    
+    /* *************************** BCE *************************** */
+    float previous_positionCmd = -1;
+    BCE_ticker.attach(&BCE_ticking, 2.0);
+    /* *************************** BCE *************************** */
+    
+    while(1) 
+    {
+        /* *************************** IMU *************************** */
+        IMU_STRING = IMU_object.IMU_run(); //grab the IMU string each iteration through the loop
+        /* *************************** 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())
+        {
+            //PC.printf("PC IS READABLE\n");  //DEBUG
+            Key=PC.getc(); 
+            if(Key=='a' or Key =='A')
+            {
+                PC.printf("%s\n", BLA_object.Keyboard_A());
+            }
+            
+            else if(Key=='d' or Key =='D')
+            {
+                PC.printf("%s\n", BLA_object.Keyboard_D());
+            }
+
+            else 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 =='Q')  //motor disabled
+            {
+                PC.printf("%s\n", BLA_object.Keyboard_Q());
+            } 
+            
+            else if(Key =='E')  //motor enabled
+            {
+                PC.printf("%s\n", BLA_object.Keyboard_E());
+            }
+            
+            else if(Key=='-' or Key =='_')
+            {
+                PC.printf("%s\n", BLA_object.Keyboard_DASH_KEY());         //decrease step size
+            } 
+            
+            else if(Key=='+' or Key=='=')
+            {
+                PC.printf("%s\n", BLA_object.Keyboard_EQUAL_KEY());        //increase step size
+            }
+            
+            else if(Key==']')  //100 to 30000, INCREASE (100 increments)
+            {
+                PC.printf("%s\n", BLA_object.Keyboard_RIGHT_BRACKET());    //motor speed
+            }
+            
+            else if(Key=='[')  //30000 to 100, DECREASE (100 increments)
+            {
+                PC.printf("%s\n", BLA_object.Keyboard_LEFT_BRACKET());     //motor speed
+            }
+            
+            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("%s\n", BLA_object.Keyboard_H());     //home linear actuator manually
+            }
+                //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
+        }
+        
+        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();
+                }
+            
+        }
+        //string snaps
+        else if (pvf().getVelocity() > 100)
+        {
+            //hBridge().stop();
+            pc().printf("********** String broke? *********\n");   
+        }
+                
+    }
+}
\ No newline at end of file