File content as of revision 0:a2b980143234:

#include "mbed.h"
#include "PID.h"

#define PULSE_PERIOD 1
#define NUM_STEPS 800

const PinName CANWritePin = p29;
const PinName CANReadPin = p30;

const int CANFrequency = 500000;
const unsigned SteeringMessageID = 0x101;
const unsigned SteeringRequestID = 0x100;

Serial pc(USBTX, USBRX);
DigitalOut led1(LED1);
DigitalOut led2(LED2);

PID SteeringController;

DigitalOut S_DIR(p24);
DigitalOut S_PUL(p23);

union { double formattedData; char rawData[8]; } CANConversion;

float steeringLocation = 0.0f;
float steeringRequest = 0.0f;

int TargetSteps = 0;

CAN CANBus(CANReadPin, CANWritePin, CANFrequency);

void AimForTarget() {
     if (TargetSteps < -5) {
        S_DIR = 0;
        S_PUL = 1;
        wait_ms(PULSE_PERIOD);
        S_PUL = 0;
        wait_ms(PULSE_PERIOD);
        TargetSteps += 1;
        //pc.printf("STEPS: %d \n\r", numSteps);
    } else if (TargetSteps > 5){
        S_DIR = 1; //CW
        S_PUL = 1;
        wait_ms(PULSE_PERIOD);
        S_PUL = 0;
        wait_ms(PULSE_PERIOD);
        TargetSteps -= 1;
        //pc.printf("STEPS: %d \n\r", numSteps);
    }
}

void UpdateTarget() {
    
    CANMessage message;
    
    if (CANBus.read(message)) {
        
        if (message.id == SteeringMessageID) {
            
            for (int i = 0; i < 8; i++) {
                CANConversion.rawData[i] = message.data[i];    
            }
    
            steeringLocation = CANConversion.formattedData;
            
            led1 = !led1;
        
        } else if (message.id == SteeringRequestID) {
            
            for (int i = 0; i < 8; i++) {
                CANConversion.rawData[i] = message.data[i];    
            }
    
            steeringRequest = CANConversion.formattedData;
            
            led2 = !led2;
            
        }
    }
    
    float error = (steeringRequest-steeringLocation);
    
    float steps = (error/360)*NUM_STEPS;
    
    TargetSteps = floor(steps);
    
    
}

int main() {
    
    pc.printf("STARTED new one\n\r");
    CANBus.attach(callback(UpdateTarget));
    
    led1 = !led1;
    
    while (1) {
        AimForTarget();
    }
    
}