File content as of revision 1:3d92e7ff3dda:

#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

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
            }
            
        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
    while (true) {
        SetMotor();
        wait(0.001f);
    }
}