Diff: main.cpp

Revision:

1:3d92e7ff3dda

Parent:

0:edcdd09960f7

Child:

2:93918cad51dd

--- a/main.cpp	Mon Oct 15 10:39:56 2018 +0000
+++ b/main.cpp	Wed Oct 17 12:23:15 2018 +0000
@@ -1,31 +1,72 @@
 #include "mbed.h"
+#include "math.h"
+#include "QEI.h"
+#include "HIDScope.h"
+
+HIDScope scope(3);
 
 AnalogIn pot1(A2); // Controls potentiometer 1, which controls motor 1
+AnalogIn pot2(A3); // Controls potentiometer 2, which is used to control Kp (position gain)
 InterruptIn button1(D0);
+
+InterruptIn motor1A(D13);   // Encoder 1a
+InterruptIn motor1B(D12);   // Encoder 1b
+QEI         Encoder1(D12,D13,NC,64);    // Note that we use X2 encoding, so there are 4200 counts/rotation, as opposed to 8400 from the documentation.
+
 DigitalOut directionpin(D7);
 PwmOut pwmpin(D6);
-
 Serial pc(USBTX, USBRX); // tx, rx
 
-void SetMotorSpeed1(){
-        float MotorSpeed1 = pot1; // Speed of the motor in rad/s
-        pc.printf("Speed of motor 1: %f rad/s. \n", MotorSpeed1);
-        if(pot1<=0.46f){
-            pwmpin = 0;
+const int CpR = 4200; //When the count is 4200, 1 revolution is made. (Using X2 encoding)
+const int MAlength = 100; // Set the length of the MA
+int MovingAverageArray[MAlength] = {pot1*CpR}; // Make an array where we can save 100 values to get an average value from our potentiometer.
+int count = 0; // Counter for the moving average array
+
+void SetMotor(){
+        int counts1 = -1*Encoder1.getPulses(); // Get counts from encoder 1 (motor 1). Notice the minus sign; CCW movement = positive counts.
+        int DesiredCounts = pot1*CpR; // The potmeter sets the desired amount of counts.
+        MovingAverageArray[count] = DesiredCounts;
+        count++;
+        
+        // define
+         int AvgCounts = 0;
+         int sum = 0;
+         for (int i = 0; i < MAlength; ++i)
+         {
+             sum += MovingAverageArray[i];
+         }
+         AvgCounts = (sum)/MAlength; //or cast sum to double before division
+        
+        if(count==MAlength){
+            count = 0; //reset counts
             }
-        else{
-        pwmpin = fabs(pot1); // Outputs the value of potmeter 1, which is used as the velocity.
-        }
-}
-
-void Motor1Direction(){
-    directionpin = !directionpin;
+            
+        scope.set(0,counts1);
+        scope.set(1,DesiredCounts);
+        scope.set(2,AvgCounts);
+        scope.send();
+        
+        // we need the following: if amount of counts by the encoder =/= desiredcounts, turn on the motor until it is equal. if aoc = dc, do nothing.
+        if(counts1 != DesiredCounts){
+            // Check which direction we have to move; when DC > AOC, we have to move CW and vice versa.
+            if(DesiredCounts>counts1){
+                directionpin=1; // still need to check whether directionpin = 1 makes the motor rotate CW.
+                }
+                else{
+                    directionpin=0;
+                }
+                pwmpin = 1;
+                pc.printf("Counts from potentiometer: %d. Average of 100 (counts): %d \n", count, AvgCounts);
+                }
+                else{
+                    pwmpin = 0; // when aoc = dc, the motor does not rotate.
+                    }
 }
 
 int main() {
     pwmpin.period_us(60);   // Set pwm period
-    button1.rise(&Motor1Direction); // if the button is pressed, change the direction
     while (true) {
-        SetMotorSpeed1();
+        SetMotor();
+        wait(0.001f);
     }
 }
\ No newline at end of file