Diff: main.cpp

Revision:

0:b26782ea38a7

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Feb 12 07:26:53 2018 +0000
@@ -0,0 +1,168 @@
+#include "mbed.h"
+#include "esc.h"
+
+DigitalOut  my_led(LED1);
+InterruptIn my_button(USER_BUTTON);
+PwmOut      m1(D3);
+PwmOut      m2(D4);
+PwmOut      m3(D5);
+PwmOut      m4(D6);
+
+
+PwmOut      m5(D7);
+PwmOut      m6(D8);
+PwmOut      m7(D9);
+
+//pwm values outputted to individual motors
+int pwm = 0;
+
+//For serial display on pc
+Serial pc(USBTX, USBRX);
+
+// Serial communication between Arduino NANO
+RawSerial device(PA_11, PA_12);  //TX RX
+
+int depth = 1;
+
+//maps value from incoming analog signal to correct range
+//to be sent out to as pwm signal to motors
+float map(float x, float in_min, float in_max, float out_min, float out_max)
+{
+  return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
+}
+
+//declare analog input pin 
+AnalogIn    joystickUD(A0);
+
+enum testStates {init, readInput, freeze, holdFreeze, holdUnfreeze} testState;
+
+void testTask(){
+    //Actions
+    switch(testState)
+    {
+        case init:
+        break; 
+        
+        case readInput:
+            //map raw joystick value
+            pwm = map(joystickUD.read(), 0.003, 0.996, 1200, 1800);
+            
+           
+       
+            //print mapped joystick values
+           // pc.printf("Pulse frequency: %d \n", pwm);  
+       
+            //send pulse in microseconds to motors
+            m1.pulsewidth_us(pwm);
+            m2.pulsewidth_us(pwm);
+            m3.pulsewidth_us(pwm);
+            m4.pulsewidth_us(pwm);
+            
+            m5.pulsewidth_us(pwm);
+            m6.pulsewidth_us(pwm);
+            m7.pulsewidth_us(pwm);
+        break;
+        
+        case freeze:
+            //send pulse in microseconds to motors
+            m1.pulsewidth_us(pwm);
+            m2.pulsewidth_us(pwm);
+            m3.pulsewidth_us(pwm);
+            m4.pulsewidth_us(pwm);
+            
+            m5.pulsewidth_us(pwm);
+            m6.pulsewidth_us(pwm);
+            m7.pulsewidth_us(pwm);
+        break;
+        
+        case holdUnfreeze:
+        break;
+        
+        case holdFreeze:
+        break;
+        
+        default:
+        break;   
+    }    
+    
+    //Transitions
+    switch(testState)
+    {
+        case init:
+            testState = readInput;
+        break;
+        
+        case readInput:
+            if (!my_button)//Button Pressed
+            {
+                testState = holdFreeze;
+            }
+            else
+            {
+                testState = readInput;
+            }
+        break;
+        
+        case freeze:
+            if (!my_button)//Button Pressed
+            {
+                testState = holdUnfreeze;
+            }
+            else //ButtonReleased
+            {
+                testState = freeze;
+            }
+        break;
+        
+        case holdFreeze:
+        if (my_button)//Button Released
+            {
+                testState = freeze;
+            }
+        else     //Button StillPressed
+            {
+                testState = holdFreeze;    
+            }
+        break;
+        
+        case holdUnfreeze:
+        if (my_button)//Button Released
+            {
+                testState = readInput;
+            }
+        else //Button StillPressed
+            {
+                testState = holdUnfreeze;    
+            }
+        break;
+        
+        default:
+        break;
+    }
+}
+
+
+
+int main()
+{
+    testState = init;
+    my_led = 0;
+    
+    while (1) {
+       
+       testTask();
+         // Read pressure sensor data if available
+            if (device.readable())
+            {
+                // Receive depth in inches as an integer
+                depth = device.getc();
+            
+                // Convert to feet
+            
+                // Display received data
+                 pc.printf("Depth: %d \n", depth);
+            }
+       wait(0.01); // 10 ms    
+       my_led =!my_led;
+    }
+}