Diff: main.cpp

Revision:

4:75f6e4845194

Parent:

3:e888f52a46bc

Child:

5:8c6d66a7c5da

--- a/main.cpp	Mon Oct 16 13:33:14 2017 +0000
+++ b/main.cpp	Thu Oct 19 09:07:43 2017 +0000
@@ -7,23 +7,30 @@
 //HIDScope scope(1);
 MODSERIAL pc(USBTX,USBRX);
 PwmOut speed1(D5);
+PwmOut speed2(D6);
 DigitalOut dir1(D4);
+DigitalOut dir2(D7);
+DigitalIn but1(PTC6);
 DigitalOut led1(D8);
 DigitalOut led2(D11);
-AnalogIn pot(A1);
-DigitalIn button1(D13);
-DigitalIn button2(D12);
+AnalogIn pot(A2);
+AnalogIn pot2(A1);
 Ticker potmeterreadout;
 Ticker encoderreadout;
 Ticker Frequency;
 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 = 6.28;//I am setting it to move through 180 degrees
+double setpoint2 = 6.28;//I am setting it to move through 180 degrees
 double Kp = 250;// you can set these constants however you like depending on trial & error
 double Ki = 100;
 double Kd = 0;
@@ -33,29 +40,39 @@
 float changeError = 0;
 float totalError = 0;
 float pidTerm = 0;
-float pidTerm_scaled = 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|
+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;
 
 
-/*void readpot()
-{
+    
 
-}
-*/
-void PIDcalculation()
+void PIDcalculation() // inputs: potvalue, motor#, setpoint
 {
     potvalue = pot.read();
     angle = motor1.getPosition()/4200.00*6.28;
     setpoint = potvalue*6.28f;
+    
+    potvalue2 = pot2.read();
+    angle2 = motor2.getPosition()/4200.00*6.28;
+    setpoint2 = potvalue2*6.28f;
     //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.001f; // derivative term
     totalError += error1*0.001f; //accumalate errors to find integral term
@@ -78,32 +95,57 @@
     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 = (Kp * error2) + (Ki * totalError2) + (Kd * changeError2);//total gain
+    pidTerm2 = pidTerm2;
+    if (pidTerm2 >= 1000) {
+        pidTerm2 = 1000;
+    } else if (pidTerm2 <= -1000) {
+        pidTerm2 = -1000;
+    } else {
+        pidTerm2 = pidTerm2;
+    }
+    //constraining to appropriate value
+        if (pidTerm2 >= 0) {
+        dir2 = 1;// Forward motion
+    } else {
+        dir2 = 0;//Reverse motion
+    }
+    pidTerm_scaled2 = abs(pidTerm2)/1000.0f; //make sure it's a positive value
+    if(pidTerm_scaled2 >= 0.55f){
+        pidTerm_scaled2 = 0.55f;
+    }
         
     last_error = error1;
     speed1 = pidTerm_scaled+0.45f;
+    last_error2 = error2;
+    speed2 = pidTerm_scaled2+0.45f;
 }
-/*
-void ReadOut()
-{
-   scope.set(0,angle);
-   scope.send(); 
-}
-*/
 
 int main()
 {
     encoderreadout.attach(PIDcalculation,0.01f);
     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);
     }