Merge pull request #7 from A2Technology/Arduino-A2-Working

Minor errors fixed

Functions in file/ON-OFFcontrol.cpp

@@ -1,4 +1,68 @@
-void control(float u, int select)
+#include <Encoder.h>
+
+#define IN1A 22
+#define IN2A 23
+#define IN1B 24
+#define IN2B 25
+#define IN1C 26
+#define IN2C 27
+#define PWM1 11
+#define PWM2 12
+#define PWM3 13
+#define EN_1A 2
+#define EN_1B 3
+#define EN_2A 18
+#define EN_2B 19
+#define EN_3A 20
+#define EN_3B 21
+
+Encoder Encoder_1(EN_1A, EN_1B);
+Encoder Encoder_2(EN_2A, EN_2B);
+Encoder Encoder_3(EN_3A, EN_3B);
+void Plot(float x, float y, float w);
+void w1(int rotation, int direct);
+void w2(int rotation, int direct);
+void w3(int rotation, int direct);
+int sign_of(float x);
+float read_speed(int select);
+void control_ONOFF(float u, int select);
+
+double x, y, w;
+int u1_sign, u2_sign, u3_sign;
+const byte speedcar = 1;
+char command;
+int scale = 1;
+int StError = 0;
+
+long oldPosition_1 = 0;
+long newPosition_1;
+long oldPosition_2 = 0;
+long newPosition_2;
+long oldPosition_3 = 0;
+long newPosition_3;
+
+long interval = 1000;
+long previousMillis = 0;
+long currentMillis = 0;
+
+long urrentEncoder;
+long previousEncoder;
+
+void setup()
+{
+    Serial.begin(9600); //Set the band rate to your Bluetooth module.
+    pinMode(IN1A, OUTPUT);
+    pinMode(IN2A, OUTPUT);
+    pinMode(IN1B, OUTPUT);
+    pinMode(IN2B, OUTPUT);
+    pinMode(IN1C, OUTPUT);
+    pinMode(IN2C, OUTPUT);
+    pinMode(PWM1, OUTPUT);
+    pinMode(PWM2, OUTPUT);
+    pinMode(PWM3, OUTPUT);
+}
+
+void control_ONOFF(float u, int select)
 {
     //control motor speed using ON-OFF control
     float k = 0.1; //Range of accepted value [u(1-k),u(1+k)]
@@ -7,13 +71,13 @@ void control(float u, int select)
     switch (select) //Select motor
     {
     case 1:
-        v = vel(1);
+        v = read_speed(1);
         break;
     case 2:
-        v = vel(2);
+        v = read_speed(2);
         break;
     case 3:
-        v = vel(3);
+        v = read_speed(3);
         break;
     }
 

Functions in file/PIDcontrol.cpp

@@ -59,14 +59,10 @@ void setup()
     PID_3.SetOutputLimits(0, 255);
 }
 
-
-
-
 void control_PID(float u, int select)
 {
     //Get sign of rotating velocity of wheels
     int u_sign = sign_of(u);
-
     switch (select)
     {
     case 1:
@@ -91,8 +87,3 @@ void control_PID(float u, int select)
         break;
     }
 }
-
-
-
-
-

Functions in file/read_velocity.cpp

@@ -1,31 +1,35 @@
-float vel(int select) 
+long interval = 1000; //choose interval is 1 second (1000 milliseconds)
+long previousMillis = 0;
+long currentMillis = 0;
+
+long currentEncoder;
+long previousEncoder;
+
+float read_speed(int select)
 {
     //read velocity of selected motor
     //return velocity in rad/s
+    const int Encoder_1_round=30 000; //define number of pulses in one round of encoder 
     switch (select)
     {
     case 1:
-        CurrentEncoder = Encoder_1.read();
+        currentEncoder = Encoder_1.read();
         break;
     case 2:
-        CurrentEncoder = Encoder_2.read();
+        currentEncoder = Encoder_2.read();
         break;
     case 3:
-        CurrentEncoder = Encoder_3.read();
+        currentEncoder = Encoder_3.read();
         break;
     }
 
+    float rot_speed;
     currentMillis = millis();
     if (currentMillis - previousMillis > interval)
     {
         previousMillis = currentMillis;
-
-        rpm = (float)abs(((CurrentEncoder - previousEncoder) * 2 * PI / ENCODEROUTPUT));
-        Serial.println(rpm);
-        previousEncoder = CurrentEncoder;
-
-        return rpm;
+        rot_speed = (float)abs(((currentEncoder - previousEncoder) * 2 * PI / Encoder_1_round));
+        previousEncoder = currentEncoder;
+        return rot_speed;
     }
-}
-
-
+}

Read_velocity/control_ONOFF.ino

@@ -47,7 +47,7 @@ long interval = 1000;
 long previousMillis = 0;
 long currentMillis = 0;
 
-long urrentEncoder;
+long currentEncoder;
 long previousEncoder;
 
 boolean measureRpm = false;
@@ -244,16 +244,17 @@ float read_speed(int select)
 {
     //read velocity of selected motor
     //return velocity in rad/s
+    const int Encoder_1_round = 30 000; //define number of pulses in one round of encoder
     switch (select)
     {
     case 1:
-        urrentEncoder = Encoder_1.read();
+        currentEncoder = Encoder_1.read();
         break;
     case 2:
-        urrentEncoder = Encoder_2.read();
+        currentEncoder = Encoder_2.read();
         break;
     case 3:
-        urrentEncoder = Encoder_3.read();
+        currentEncoder = Encoder_3.read();
         break;
     }
 
@@ -262,8 +263,8 @@ float read_speed(int select)
     if (currentMillis - previousMillis > interval)
     {
         previousMillis = currentMillis;
-        rot_speed = (float)abs(((urrentEncoder - previousEncoder) * 2 * PI / ENCODEROUTPUT));
-        previousEncoder = urrentEncoder;
+        rot_speed = (float)abs(((currentEncoder - previousEncoder) * 2 * PI / Encoder_1_round));
+        previousEncoder = currentEncoder;
         return rot_speed;
     }
 }