File content as of revision 19:8746a2c663f8:

#include "mbed.h"
#include "MODSERIAL.h"
#include "encoder.h"
#include "math.h"
#include "HIDScope.h"

//HIDScope scope(1);
MODSERIAL pc(USBTX,USBRX);
AnalogOut servospeed();
PwmOut speed1(D5);
PwmOut speed2(D6);
DigitalOut dir1(D4);
DigitalOut dir2(D7);
DigitalIn press(PTA4);
DigitalOut led1(D8);
DigitalOut led2(D11);
AnalogIn pot(A2);
AnalogIn pot2(A3);
Ticker mainticker;
Encoder motor1(PTD0,PTC4);
Encoder motor2(D12,D13);

float PwmPeriod = 0.0001f;

double count = 0; //set the counts of the encoder
volatile double angle = 0;//set the angles

double count2 = 0; //set the counts of the encoder
volatile double angle2 = 0;//set the angles

double setpoint = 0;//I am setting it to move through 180 degrees
double setpoint2 = 0;//I am setting it to move through 180 degrees

double Kp = 15.25;// you can set these constants however you like depending on trial & error
double Ki = 0.0;
double Kd = 0.0;

double Kp2 = 30;
double Ki2 = 30;
double Kd2 = 2;

float last_error = 0;
float error1 = 0;
float changeError = 0;
float totalError = 0;
float pidTerm = 0;
float pidTerm_scaled = 0;

float last_error2 = 0;
float error2 = 0;
float changeError2 = 0;
float totalError2 = 0;
float pidTerm2 = 0;
float pidTerm_scaled2 = 0;// if the total gain we get is not in the PWM range we scale it down so that it's not bigger than |255|

volatile double potvalue = 0.0;
volatile double potvalue2 = 0.0;
volatile double position = 0.0;
volatile double position2 = 0.0;

//bool readoutsetpoint = true;

void setpointreadout()
{
   
    potvalue = pot.read();
    setpoint = potvalue*6.28f*15.0f;
    
    //potvalue2 = pot2.read();
    setpoint2 = potvalue*6.28f*15.0f;
   
}
    

void PIDcalculation() // inputs: potvalue, motor#, setpoint
{
    setpointreadout();
    angle = motor1.getPosition()/4200.00*6.28;   
    angle2 = motor2.getPosition()/4200.00*6.28;
   
    //pc.printf("pos: %d, speed %f reference position = %.2f\r ",motor1.getPosition(), motor1.getSpeed(), setpoint);
    //motorpid = PID(potvalue - position, M1_KP, M1_KI, M1_KD, M1_TS, m1_err_int, m1_prev_err, m1_f_v1, m1_f_v2, M1_F_A1, M1_F_A2, M1_F_B0, M1_F_B1, M1_F_B2);
    
    error1 = setpoint - angle;
    error2 = setpoint2 - angle2;
    
    changeError = (error1 - last_error)/0.002f; // derivative term
    totalError += error1*0.002f; //accumalate errors to find integral term
    pidTerm = (Kp * error1) + (Ki * totalError) + (Kd * changeError);//total gain
    pidTerm = pidTerm;
    
    if (pidTerm >= 100) {
        pidTerm = 100;
    } else if (pidTerm <= -100) {
        pidTerm = -100;
    } else {
        pidTerm = pidTerm;
    }
    //constraining to appropriate value
        if (pidTerm >= 0) {
        dir1 = 1;// Forward motion
    } else {
        dir1 = 0;//Reverse motion
    }
    pidTerm_scaled = abs(pidTerm)/100.0f; //make sure it's a positive value
    if(pidTerm_scaled >= 0.55f){
        pidTerm_scaled = 0.55f;
    }
    
    changeError2 = (error2 - last_error2)/0.001f; // derivative term
    totalError2 += error2*0.001f; //accumalate errors to find integral term
    pidTerm2 = (Kp2 * error2) + (Ki2 * totalError2) + (Kd2 * changeError2);//total gain
    pidTerm2 = pidTerm2;
    if (pidTerm2 >= 100) {
        pidTerm2 = 100;
    } else if (pidTerm2 <= -100) {
        pidTerm2 = -100;
    } else {
        pidTerm2 = pidTerm2;
    }
    //constraining to appropriate value
        if (pidTerm2 >= 0) {
        dir2 = 1;// Forward motion
    } else {
        dir2 = 0;//Reverse motion
    }
    pidTerm_scaled2 = abs(pidTerm2)/100.0f; //make sure it's a positive value
    if(pidTerm_scaled2 >= 0.55f){
        pidTerm_scaled2 = 0.55f;
        
    }
        
    last_error = error1;
    last_error2 = error2;
    
 
        speed1 = pidTerm_scaled+0.45f;
   
        speed2 = pidTerm_scaled2+0.45f;
    
    
}

int main()
{
    mainticker.attach(PIDcalculation,0.002f);
    speed1.period(PwmPeriod);
    speed2.period(PwmPeriod);
    
    int count = 0;    
    while(true) {
        
        count++;
        if(count == 100){
            count = 0;
            pc.printf("Angle = %f, pidTerm = %f ,PID_scaled = %f, Error = %f, setpoint = %f\r\n", angle, pidTerm ,pidTerm_scaled, error1,setpoint);
            //pc.printf("Angle 2 = %f, pidTerm 2 = %f ,PID_scaled 2 = %f, Error 2 = %f, setpoint 2 = %f\r\n", angle2, pidTerm2 ,pidTerm_scaled2, error2,setpoint2);
        }
         
         
    wait(0.001f);
    }
       
            
            
        
        
    

}