File content as of revision 6:ee243782bf51:

#include "mbed.h"
#include "FastPWM.h"
#include "HIDScope.h"
#include "QEI.h"
#include "MODSERIAL.h"
#define SERIAL_BAUD 115200
 
MODSERIAL pc(USBTX,USBRX);
// Set button pins
InterruptIn button1(PTB9);
InterruptIn button2(PTA1);

// Set AnalogIn potmeters
AnalogIn pot1(A3);
AnalogIn pot2(A4);

// Set motor Pinouts
DigitalOut motor1_dir(D4);
FastPWM motor1_pwm(D5);
DigitalOut motor2_dir(D7);
FastPWM motor2_pwm(D6);

// Set LED pins
DigitalOut led(LED_RED);

// Set HID scope
HIDScope    scope(2);

// Set Encoders for motors
QEI Motor1_ENC_CW(D11,D10,NC,32);
QEI Motor1_ENC_CCW(D10,D11,NC,32);
QEI Motor2_ENC_CW(D12,D13,NC,32);
QEI Motor2_ENC_CCW(D13,D12,NC,32);

// Constants
volatile double pwm = 0.0;
volatile double pwm_new = 0;
volatile double timer = 0.0;
volatile bool button1_value = false;
volatile bool button2_value = false;

// Ticker
Ticker SinTicker;
Ticker PotControlTicker;
Ticker SendZerosTicker;

void SwitchLed()
{
    led = not led;
}

void button1_switch()
{
    button1_value = not button1_value;
}

void button2_switch()
{
    button2_value = not button2_value;
}

void SendZeros()
{
    int zero=0;
    scope.set(0,zero);
    scope.set(1,zero);
    scope.send();
}

void SetMotor(int motor_number, double MotorValue)
{
    // Given -1<=motorValue<=1, this sets the PWM and direction
    // bits for motor 1. Positive value makes motor rotating
    // clockwise. motorValues outside range are truncated to
    // within range
    if (motor_number == 1)
    {
        if (MotorValue >=0)
        {
            motor1_dir=1;
        }
        else
        {
            motor1_dir=0;
        }
        if (fabs(MotorValue)>1){
            motor1_pwm.write(1);
        }
        else
        {
            motor1_pwm.write(abs(MotorValue));
        }
    }
    else
    {  
        if (MotorValue >=0)
        {
            motor1_dir=1;
        }
        else
        {
            motor1_dir=0;
        }
        if (fabs(MotorValue)>1){
            motor1_pwm.write(1);
        }
        else
        {
            motor1_pwm.write(abs(MotorValue));
        }
    }
        
}

double GetPosition(int motor_number)
{
    const int   COUNTS_PER_REV = 8400;
    const float DEGREES_PER_PULSE = 8400 / 360;
    if (motor_number == 1)
    {
        int countsCW = Motor1_ENC_CW.getPulses();
        int countsCCW= Motor1_ENC_CCW.getPulses();
        int net_counts = countsCW-countsCCW;
        double Position=(net_counts*360.0f)/COUNTS_PER_REV;
        return Position;
    }
    else
    {
        int countsCW = Motor2_ENC_CW.getPulses();
        int countsCCW= Motor2_ENC_CCW.getPulses();
        int net_counts = countsCW-countsCCW;
        double Position=(net_counts*360.0f)/COUNTS_PER_REV;
        return Position;
    }
}

void MultiSin_motor1()
{
    double pwm = pwm_new;
    double f1 = 0.2f;
    double f2 = 1.0f;
    double f3 = 5.0f;
    double f4 = 20.0f;
    double f5 = 100.0f;
    const double pi = 3.141592653589793238462;
    // read encoder
    double motor1_position = GetPosition(1);
    // set HIDSCOPE values, position, pwm*volt
    scope.set(0, motor1_position);
    scope.set(1, pwm);
    // sent HIDScope values
    scope.send();
    // calculate the new multisin
    double multisin = sin(2*pi*f1*timer)+sin(2*pi*f2*timer)+sin(2*pi*f3*timer)+sin(2*pi*f4*timer)+sin(2*pi*f5*timer);
    // devide by the amount of sin waves
    pwm_new=multisin / 5;
    // write the motors
    SetMotor(1, multisin);
    // Increase the timer
    timer = timer + 0.001;
    if (timer >= (20.0-0.001))
    {
        SinTicker.detach();
        timer = 0;
    }
}

void MultiSin_motor2()
{      
    double pwm = pwm_new;
    double f1 = 0.2f;
    double f2 = 1.0f;
    double f3 = 5.0f;
    double f4 = 20.0f;
    double f5 = 100.0f;
    const double pi = 3.141592653589793238462;
    // read encoder
    double motor2_position = GetPosition(2);
    // set HIDSCOPE values, position, pwm*volt
    scope.set(0, motor2_position);
    scope.set(1, pwm);
    // sent HIDScope values
    scope.send();
    // calculate the new multisin
    double multisin = sin(2*pi*f1*timer)+sin(2*pi*f2*timer)+sin(2*pi*f3*timer)+sin(2*pi*f4*timer)+sin(2*pi*f5*timer);
    // devide by the amount of sin waves
    pwm_new=multisin / 5;
    // write the motor
    SetMotor(2, multisin);
    // Increase the timer
    timer = timer + 0.001;
    if (timer >= (20.0-0.001))
    {
        SinTicker.detach();
        timer = 0;
    }
}

void PotControl()
{
    double Motor1_Value = (pot1.read() - 0.5)/2.0;
    double Motor2_Value = (pot2.read() - 0.5)/2.0;
    //pc.printf("\r\n Motor value 1: %f \r\n",Motor1_Value);
    //pc.printf("\r\n Motor value 2: %f \r\n",Motor2_Value);
    scope.set(0, Motor1_Value);
    scope.set(1, Motor2_Value);
    scope.send();
    // Write the motors
    SetMotor(1, Motor1_Value);
    SetMotor(2, Motor2_Value);
}

void SinControl1()
{
    SinTicker.attach(&MultiSin_motor1, 0.001);
}

void SinControl2()
{
    SinTicker.attach(&MultiSin_motor2, 0.001);
}

void HIDScope_Send()
{
    scope.send();
}



int main() {
    pc.baud(SERIAL_BAUD);
    pc.printf("\r\n ***THERMONUCLEAR WARFARE HAS COMMENCED*** \r\n");
    
    // Set LED
    led=0;
    Ticker LedTicker;
    LedTicker.attach(SwitchLed,0.5f);    
    
    // Interrupt for buttons
    button1.fall(button1_switch);
    button2.fall(button2_switch);
    
    pc.printf("\r\n ***************** \r\n");
    pc.printf("\r\n Press button 1 to start positioning the arms \r\n");
    pc.printf("\r\n Make sure both potentiometers are positioned halfway! \r\n");
    pc.printf("\r\n ***************** \r\n");
    while (button1_value == false)
    {
        // just wait
    }
    pc.printf("\r\n ***************** \r\n");
    pc.printf("\r\n Use potentiometers to control the motor arms \r\n");
    pc.printf("\r\n Pot 1 for motor 1 \r\n");
    pc.printf("\r\n Pot 2 for motor 2 \r\n");
    pc.printf("\r\n ***************** \r\n");
    pc.printf("\r\n Press button 2 to start the rest of the program \r\n");
    pc.printf("\r\n ***************** \r\n");
    // Start potmeter control
    PotControlTicker.attach(&PotControl,0.01f);
    
    while (button2_value == false)
    {
        // just wait   
    }
    PotControlTicker.detach();
    SendZerosTicker.attach(&SendZeros,0.001);
    
    // Program startup   
    pc.printf("\r\n Starting multisin on motor 1 in: \r\n");
    pc.printf("\r\n 10 \r\n");
    wait(5);
    pc.printf("\r\n 5 \r\n");
    wait(1);
    pc.printf("\r\n 4 \r\n");
    wait(1);
    pc.printf("\r\n 3 \r\n");
    wait(1);
    pc.printf("\r\n 2 \r\n");
    wait(1);
    pc.printf("\r\n 1 \r\n");
    wait(1);
    pc.printf("\r\n 0 \r\n");
    pc.printf("\r\n Wait for the signal to complete \r\n");
    pc.printf("\r\n Press button 2 too continue afterwards \r\n");
    SendZerosTicker.detach();
    SinControl1();

    
    while (button2_value == true)
    {
        // just wait   
    }
    
    
    SendZerosTicker.attach(&SendZeros,0.001);
    pc.printf("\r\n Starting multisin on motor 2 in: \r\n");
    pc.printf("\r\n 10 \r\n");
    wait(5);
    pc.printf("\r\n 5 \r\n");
    wait(1);
    pc.printf("\r\n 4 \r\n");
    wait(1);
    pc.printf("\r\n 3 \r\n");
    wait(1);
    pc.printf("\r\n 2 \r\n");
    wait(1);
    pc.printf("\r\n 1 \r\n");
    wait(1);
    pc.printf("\r\n 0 \r\n");
    SendZerosTicker.detach();
    SinControl2();
    wait(23);
    pc.printf("\r\n Program Finished, press reset to restart \r\n");
    while (true) {}
}