File content as of revision 4:8113394bed1b:

/*
    The Poolinator - A pool playing robot for people with DMD

GROUP 10
Sjoerd de Jong              - s1949950
Joost Loohuis               - s1969633
Viktor Edlund               - s2430878
Giuseppina Pinky Diatmiko   - s1898841
Daan v.d Veen               - s2003171
*/

// THE PINOUT IS SHOWN IN THE 'PINOUT' DOC ON THE DRIVE!! //

#include "mbed.h"
#include "HIDScope.h"
#include "QEI.h"
#include "MODSERIAL.h"
#include "BiQuad.h"
#include "FastPWM.h"
#include "Servo.h"

MODSERIAL pc(USBTX, USBRX);

AnalogIn pot(A1);

// DC MOTORS CODE //
QEI encoder1(D12,D13,NC,32), encoder2(D10,D11,NC,32), encoder3(D8,D9,NC,32);
PwmOut motor1_pwm(D6), motor2_pwm(D5), motor3_pwm(D3);
DigitalOut motor1_dir(D7), motor2_dir(D4), motor3_dir(D2);
int frequency_pwm = 10000;

void setMotor(int motor, float motor_spd) //
{
    int motor_dir = (motor_spd >= 0) ? 0 : 1;

    motor_spd = fabs(motor_spd);
    if(motor_spd > 1) motor_spd = 1;
    if(motor_spd < 0.1) motor_spd = 0; // Don't turn if too small
    else motor_spd = (motor_spd * 0.5) + 0.5; // else, map between 0.5 and 1, motor doesn't turn when smaller

    if(motor == 1) {
        motor1_dir.write(motor_dir);
        motor1_pwm.write(motor_spd);
    } else if(motor == 2) {
        motor2_dir.write(motor_dir);
        motor2_pwm.write(motor_spd);
    } else if(motor == 3) {
        motor3_dir.write(motor_dir);
        motor3_pwm.write(motor_spd);
    }
}



// PID CODE //
double Kp = 5.5, Ki = 1.02, Kd = 0.0; // No Kd, makes the motor wiggle back and forth, dunno why (default: 17.5, 1.02, 23.2)
double Ts = 0.01; // Sample time in seconds
double PID_gain = 0.00075; // Because we use counts instead of radians, 1/8400*2PI=0.00075

static double error_integral = 0;
static double error_prev = 0;
static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);

double pid(double error)
{
    // Proportional part:
    double u_k = Kp * error;

    // Integral part
    error_integral = error_integral + error * Ts;
    if(error > 1000) error_integral = 0; // Only apply the I-action if the error is small, otherwise speed will keep building up and cause big overshoots
    double u_i = Ki * error_integral;

    // Derivative part
    double error_derivative = (error - error_prev) / Ts;
    double filtered_error_derivative = LowPassFilter.step(error_derivative);
    double u_d = Kd * filtered_error_derivative;
    error_prev = error;

    // Sum all parts and return it
    return u_k + u_i + u_d;
}



// STEPPER MOTOR CODE //
DigitalOut STEPPER_IN1(PTB18), STEPPER_IN2(PTB19), STEPPER_IN3(PTC1), STEPPER_IN4(PTC8);

int stepper_steps = 0;
float stepper_angle = 0;

void stepper_move(int direction_) // Requires ~1.5ms wait time between each step
{
    STEPPER_IN1 = (stepper_steps == 5 || stepper_steps == 6 || stepper_steps == 7);
    STEPPER_IN2 = (stepper_steps == 3 || stepper_steps == 4 || stepper_steps == 5);
    STEPPER_IN3 = (stepper_steps == 1 || stepper_steps == 2 || stepper_steps == 3);
    STEPPER_IN4 = (stepper_steps == 7 || stepper_steps == 0 || stepper_steps == 1);
    stepper_steps += (direction_ == 0) ? - 1 : 1;
    stepper_steps = (stepper_steps + 8) % 8;
    stepper_angle += ((direction_ == 0) ? -1 : 1) * (360.00 / 4096.00);
}



// SERVO CODE //
Servo myservo(PTC9);



// SOLENOID CODE //
DigitalOut solenoidA(PTC0), solenoidB(PTC7);

void setSolenoid(int dir)
{
    solenoidA = (dir == 0) ? 0 : 1;
    solenoidB = (dir == 0) ? 1 : 0;
}




int main()
{
    pc.baud(115200);
    pc.printf("\r\nStarting...\r\n\r\n");
    motor1_pwm.period(1.0 / frequency_pwm); // T = 1 / f
    motor2_pwm.period(1.0 / frequency_pwm);
    motor3_pwm.period(1.0 / frequency_pwm);

    while (true) {
        /*
        SOME EXAPLE CODE
        
        * MOTOR
        setMotor(..., ...) // which motor (1,2,3), and speed (-inf, +inf)
        
        * PID
        double currentPos1 = encoder1.getPulses();
        double targetPos1 = 10000;
        double error1 = targetPos1 - currentPos1;
        float PID_speed1 = pid(error) * PID_gain;
        setMotor(1, PID_speed1);

        * STEPPER
        stepper_move(1); // direction (0 or 1)
        wait_ms(1.5); // requires some time between steps

        * SERVO
        myservo = ... // value between 0.0 and 1.0
        
        * SOLENOID 
        setSolenoid(0); // shooting
        wait(1000);
        setSolenoid(1);
        
        */
      
    }
}