File content as of revision 2:825571dce613:

/*
  Sailbot Hull 14
  L Marino and D Robinson,
  with D Evangelista, P Frontera, M Kutzer, A Laun
  2018

  Threaded version using mbed OS5
  Serial links between nodes
  Stepper motors with Pololu 2968 drivers (MP6500 based)
  Piher PST360 angle sensors
  Adafruit Absolute GPS
  Telemetry via ???Ubiquity???
*/

//Thread for logging time and Gps, heading and encoder values is mostly complete
//control thread is a work in progress

#include "mbed.h"
#include <stdio.h>
#include <stdlib.h>
#include <QEI.h>

DigitalOut led1(LED1);
DigitalOut led2(LED2);
DigitalOut spin(p20);
DigitalOut fire(p19);
PwmOut pwm(p25);
DigitalOut dir(p27);
Serial pc(USBTX,USBRX);
QEI turret(p17, p18, NC, 1600);
Timer t;

int qq;
float pulses;
float revs;
float dc;
float theta;
float err = 0.0;
float err_old = 0.0;
float err_ancient = 0.0;
float dc_old = 0.0;
float dc_ancient = 0.0;
float des_theta=0.0;
float t_now;
int ii=0;
int shots;
int i;

//float t;
//float t_2;
int yes;

Ticker Controller;
void CtrCode()
{
    pulses = turret.getPulses();
    revs = -pulses / 3200.0;
    theta = revs*360.0;

    err = des_theta-theta;

    //dc = 1.9703*dc_old - 0.9703*dc_ancient + .1471*err- 0.293*err_old + 0.146*err_ancient;    //1.9703*dc_old - 0.9703*dc_ancient + .1471*err - 0.293*err_old + 0.146*err_ancient;
    dc = 1.9703*dc_old - 0.9703*dc_ancient + 0.7918*err - 1.5762*err_old + 0.7844*err_ancient;

    err_ancient = err_old;
    err_old = err;
    dc_ancient = dc_old;
    dc_old = dc;

    if (dc>0) {
        dir = 1;
    } else {
        dir = 0;
    }

//Filter for derivative
    if(t.read()<=3.0) {
        pwm.write(abs(dc));
    }
    if(t.read()>3.0) {
        if(abs(err)<=0.50) {
            pwm.write(0);
        } else {
            pwm.write(abs(dc));
        }
    }
}


void FireCode()
{
    
    while(1) {
        int counter = 0;
        while(abs(err)<=0.50) {
            Thread::wait(5);
            ii++;
            if(ii>15) {

                spin = 1;
                led1 = 1;
                Thread::wait(3000);
                
                while(counter<shots) {
                    fire = 1;
                    led2 = 1;
                    Thread::wait(100);
                    led2 = 0;
                    fire = 0;
                    Thread::wait(7500);
                    counter = counter + 1;

                }
                spin = 0;
                led1 = 0;
            }
        }
    }
}

// *****threading*****
Thread Spin_thread;
Thread Shoot_thread;


void Spin_callback(void);
void Shoot_callback(void);

int main()
{

    float pwm_now;

    pc.baud(9600); // set baud rate
    dir=1;
    pwm.period(1.0/(20*10^3));
    pc.scanf("%f,%d",&des_theta,&shots); //scan in desired theta and # of shots from Tera term
    qq = 0;
    while(qq<shots) {
        led2 = 1;
        Thread::wait(750);
        led2 = 0;
        qq++;
    }
    t.start();
    //start all threads
    Thread::wait(1000);
    Spin_thread.start(callback(Spin_callback));
    Shoot_thread.start(callback(Shoot_callback));

    while(t.read()<=25) {
        t_now = t.read();
        pc.printf("Time: %f, Error: %f, DC: %f\n", t_now,err, dc);
    }

    t.stop();
    //could have a heart beat but I tried to reduce the number of threads fighting for time
}//end main

void Spin_callback(void)
{

    Controller.attach(&CtrCode,.0083);
}

void Shoot_callback(void)
{
    FireCode();

}