motor_arm_PID.cpp

#include "mbed.h"
#include "rtos.h"

/* the pins that will be used on the Nucleo board */
AnalogIn analog_value(A0);
PwmOut mypwm(D10);
DigitalOut directionPin(D2);
DigitalOut enablePin(D3);
DigitalOut led1(LED1);

/* some handy globals */
int setpoint = 10000;
int currentpos;
int delaytime  = 20;
int previouserror = 0;

/* task handles PID calculations and PWM outputs to the motor */
void positionTask_thread(void const *argument ){
    while (1){
    int Kp = 10;
    int Ki = 1;
    int Kd = 5;
    int integral = 0;
    
    int error = setpoint - currentpos;
    integral = integral + error*delaytime;
    int derivative = (error - previouserror)/delaytime;
    int pwmoutput = Kp*error + Ki*integral + Kd*derivative;
    previouserror = error;
    printf("Previous Error = %d \n\r", setpoint);
    if(pwmoutput > 0) directionPin = 0;
    else directionPin = 1;
    
    if(pwmoutput < 0) pwmoutput = (pwmoutput * -1);
    if(pwmoutput > 65000/4) pwmoutput = 65000/4;
  
    mypwm.pulsewidth_us(pwmoutput);
    Thread::wait(delaytime);
    }
}

/* task handles reading the ADC value and storing it globally */
void getPosition_thread(void const *argument){
    while(1){
      int meas;
      meas = analog_value.read_u16();
      currentpos = meas/4; 
      Thread::wait(delaytime - 1);
    }      
}

/* prints values for user, this thread can be removed */
void printValues_thread(void const *argument){
  while(1){
    printf("Position: %d \n\r", currentpos);
    printf("Error: %d \n\r", previouserror);
    printf("PWM Duty: %d \n\r", mypwm.read());
    Thread::wait(200);
  }
}

/* blinks the LED on the board */
void blink_thread(void const *argument)
{
    while (1) {
        Thread::wait(500);
	led1 = !led1;
    }
}

int main()
{
    enablePin = 1;
    Thread thread(blink_thread, NULL, osPriorityNormal, DEFAULT_STACK_SIZE);
    Thread thread1(positionTask_thread, NULL, osPriorityNormal, DEFAULT_STACK_SIZE);
    Thread thread2(printValues_thread, NULL, osPriorityNormal, DEFAULT_STACK_SIZE);
    Thread thread3(getPosition_thread, NULL, osPriorityNormal, DEFAULT_STACK_SIZE);
    while (true) {
        led1 = !led1;
        Thread::wait(5000);
	setpoint = (rand()%11000) + 3700;
	
    }
}