Diff: main.cpp

Revision:

0:3c2e28fef203

Child:

1:b56499238c53

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Oct 11 10:43:31 2019 +0000
@@ -0,0 +1,179 @@
+#include "mbed.h"
+//#include "HIDScope.h"
+//#include "QEI.h"
+#include "MODSERIAL.h"
+//#include "BiQuad.h"
+#include "FastPWM.h"
+#include <iostream>
+#include <math.h>
+
+//COMMUNICATING WITH PC
+#define SERIAL_BAUD 115200
+Serial pc(USBTX, USBRX);
+
+Ticker EncTicker; // Ticker for encoder
+
+//MOTOR 1 //DONT HAVE TO BE CONNECTED BY WIRES, IS DONE BY THE HARDWARE ITSELF
+DigitalOut direction_1(D7); //tells motor 1 to turn CW (bool = false) or CCW (bool = true)
+PwmOut msignal_1(D6); //PWM signal to motor controller, higher PWM = higher avarage voltage
+DigitalIn but_1(D1); //D1 to BUT1
+DigitalIn enc_1a(D10); //D13 to ENC1 A
+DigitalIn enc_1b(D9); //D12 to ENC1 B
+
+//MOTOR 2
+//DigitalOut direction_2(D4); //tells motor 2 to turn CW or CCW
+//PwmOut msignal_2(D5); //PWM signal to motor controller, higher PWM = higher avarage voltage
+
+volatile float width;
+volatile float width_percent;
+float period = 10.0; //sets period to 10ms
+volatile float on_time_1;
+volatile float off_time_1;
+volatile bool dir_1; //True = CCW, false = CW
+volatile float a_1 = enc_1a; // enc_a1 signal
+volatile float b_1 = enc_1b; // enc b_1 signal
+volatile float a_1prev = enc_1a; // enc_a1 previous signal
+volatile float b_1prev = enc_1b; // enc b_1 previous signal
+volatile int ticks_1 = 0; //counts times the encoder ticked from the start position
+volatile float psi_1 = 0; //angle of motor_1 in radians
+volatile float psi_1prev = 0; //precious angle of motor_1 in radians
+#define M_PI acos(-1.0)
+const float to_rads =((2*M_PI)/8400); //contant for claculating one tick of the motor to radians, 8400 is a property of the motor -> 8400 counts per revolution
+
+// FUNCTIONS
+float GetValueFromKeyboard() //Gets input from PC
+{
+    start:
+    
+    float a;
+    cin >> a;
+    if(a>1)
+    {
+        pc.printf("\nYour input was invalid, please imput a value in interval (-1,1).\r\n");
+        goto start;
+        }
+    else if(a<-1)
+    {
+        pc.printf("\nYour input was invalid, please imput a value in interval (-1,1).\r\n");
+        goto start;
+        }
+    else if(a<0)
+    {
+        pc.printf("\nYour input: [%f] -> CW motion\r\n\r\n--------\r\n\n",a);
+        dir_1 = false;
+        a = abs(a); //otherwise width becomes negative, and it doesnt like that
+        return a;
+        }
+    else
+    {
+        pc.printf("\nYour input: [%f] -> CCW motion\r\n\r\n--------\r\n\n",a);
+        dir_1 = true;
+        return a;
+        }
+}
+
+//void GetDirectionFromkeyboard ()
+//{
+//    start_dir:
+//    
+//    int b;
+//    cin >> b;
+//    switch (b)
+//    {
+//        case 1:
+//        {
+//            pc.printf("\nYour input: [1] -> CCW motion\r\n\r\n--------\r\n\n");
+//            dir_1 = true;
+//            break;
+//            }
+//        case 0:
+//        {
+//            pc.printf("\nYour input: [0] -> CW motion\r\n\r\n--------\r\n\n");
+//            dir_1 = false;
+//            break;
+//            }
+//        default:
+//        {
+//            pc.printf("\nYour input was invalid, please input 1 for CCW or 0 for CW motion.\r\n");
+//            goto start_dir;
+//            }
+//    }
+//}
+
+void PWM_MOTOR_1 (void) //Calculates on and off times
+{
+    width = period * width_percent;
+    on_time_1 = width;
+    off_time_1 = period - width;
+    }
+
+void MOTOR_1 () //runs the motor
+{
+    msignal_1 = 1; // Turn motor on
+    wait_ms(on_time_1);
+    msignal_1 = 0; // Turn motor off
+    wait_ms(off_time_1); 
+    }
+    
+void ReadEnc_1() 
+{ 
+    a_1prev = a_1;
+    b_1prev = b_1;
+    a_1 = enc_1a;
+    b_1 = enc_1b;
+
+    //pc.printf("a_1i = %f, b_1i = %f\r\n", a_1, b_1);
+    }
+
+float Enc_1Radians ()
+{
+    if(a_1==a_1prev && b_1==b_1prev)
+    {
+        }
+    else
+    {
+        psi_1prev = psi_1;
+        if(dir_1)
+        {
+            psi_1 = psi_1prev + to_rads;
+            }
+        else
+        {
+            psi_1 = psi_1prev - to_rads;
+            }
+        pc.printf("Angle of motor_1: %f radians\r\n",psi_1);
+        }
+    return psi_1;
+    }
+
+int main()
+{   
+    pc.baud(SERIAL_BAUD);
+    
+    pc.printf("--------\r\nWelcome!\r\n--------\r\n\n");
+    EncTicker.attach(&ReadEnc_1, 0.0001); //Ticker function to read Enc_1 to HIDScope
+    
+    start_main:
+    
+//    pc.printf("Please imput your desired direction value, either 1 for CCW or 0 for CW movement\r\n");
+//    GetDirectionFromkeyboard();
+     
+    
+    wait(0.5);
+    
+    pc.printf("Please imput your <width> value in percentile of the period (-1,1), positive for CCW motion or negative for CW motion\r\n");
+    width_percent = GetValueFromKeyboard(); //in percentile (-1,1)
+    direction_1 = dir_1;
+    PWM_MOTOR_1();      
+    
+    while (true) 
+    {
+        Enc_1Radians ();
+        MOTOR_1();
+        if (but_1 == 0)
+        {
+            pc.printf("--------\r\nTurning off motor...\r\n--------\r\n\n");
+            goto start_main;
+            }
+    }
+}