Diff: main.cpp

Revision:

22:c18f04d1dc49

Parent:

21:eac29cf3f061

Child:

23:5238b046119b

--- a/main.cpp	Wed Nov 16 23:48:46 2016 +0000
+++ b/main.cpp	Fri Nov 18 02:47:59 2016 +0000
@@ -1,8 +1,9 @@
 #include "QEI.h"
 #include "Motor.h"
 #include "pot.h"
+#include "list.h"
 #include <time.h>
-
+#include "pid.h"
  
 Serial pc(USBTX, USBRX);
 //Use X4 encoding.
@@ -11,6 +12,7 @@
 QEI encoder (PTC16, PTC17, PTB9, 512);
 Motor motor(PTC4, PTD0, PTC3, PTC12, PTB23, 2000);
 Pot pend(PTC10, &encoder,0);
+PID pid = PID(pend.UPDATE_TIME, 4700, -4700, 500, 0, 0); 
 Ticker update;
 
 void test_distance(){
@@ -43,39 +45,12 @@
     }       
 }
 
-void test_speed() {
-    time_t start;
-    double elapsed_time = 0;
-    int steps = 10;
-    int wait = 1000;
-    printf("steps: %i\r\n", steps);
-            for( int i = 0; i < 3; i++){
-                encoder.reset();
-                start = time(0);
-                motor.step_clockwise(steps, wait);
-                elapsed_time = difftime( time(0), start);
-                printf("Round %i clockwise, Total duration: \t%f seconds\r\n", i, elapsed_time);
-                wait_ms(200);
-                
-                encoder.reset();
-                start = time(0);
-                motor.step_anticlockwise(steps, wait);
-                elapsed_time = difftime( time(0), start);
-                printf("Round %i counter-clockwise, Total duration: \t%f seconds\r\n", i, elapsed_time);
-                wait_ms(200);
-            }
-}
 
 
-void test_pendulum(){
-    while(1){
-        printf("angle: %f \r\n", pend.get_angle());   
-    }
-}
 void calibrate_pendulum(){
     encoder.reset();
     while(1){
-        printf("voltage %%: %f  pulse: %i", pend.angle_as_voltage(), encoder.getPulses());
+        printf("voltage %%: %f  pulse: %i\r\n", pend.angle_as_voltage(), encoder.getPulses());
     }
 }
 void move_pulses(int pulses, int wait){ // find number of pulses from the encoder going from one end to the other.
@@ -90,15 +65,66 @@
         }
     } 
 }
+void test_speed(){
+    int speed = 1000;
+    int decrement = 10;
+ 
+    List velocities_cw = List(1000);
+    List velocities_ccw = List(1000);
+    printf("hi");
+    while(speed > 400){
+        for(int i =0; i<100; i++){
+            motor.clockwise(speed);
+            velocities_cw.add(pend.velocity);
+        }
+        wait(1);
+        for(int i =0; i<100; i++){
+            motor.anticlockwise(speed);
+            velocities_ccw.add(pend.velocity);
+        }
+        wait(1);
+        printf("clockwise velocity peak: %f avg: %f anticlockwise velocity peak: %f avg: %f \r\n"
+         ,velocities_cw.min(), velocities_cw.average(),  velocities_ccw.max(), velocities_ccw.average());
+        speed -= decrement;
+    }       
+}
 
 void update_handler(){
     pend.update();
+    double output = pid.calculate(180, pend.angle);
+    //printf("output: %f angle: %f\r\n",output , pend.angle);
+    if(output > 0){ 
+        motor.wait = 5000 - (int) output;
+        motor.dir = false;
+    }else{
+        motor.wait = 5000 + (int) output;
+        motor.dir = true;
+    }  
 }
 int main(){
-    int neg = -1;
-    encoder.reset();
+    wait(2);
+    pend.set_zeros();
     update.attach(&update_handler, pend.UPDATE_TIME);
-    pend.set_zeros();
+    while(1){
+        //pend.print_test();
+        motor.run(true);
+        //calibrate_pendulum();
+    }
+    
+    
+    
+  
+    
+   /* while(1){
+        
+        //printf("%f\r\n",pend.angle_as_voltage());
+        //pend.print_test();
+        if(pend.angle>2) motor.anticlockwise(1000);
+        else if( (pend.angle>=-2) && (pend.angle<=2))wait_ms(1);
+        else motor.clockwise(1000);
+    }
+
+   pend.set_zeros();
     int waits = 700;
     while(1){
         move_pulses(1000*neg, waits);
@@ -110,10 +136,10 @@
         wait(3);
     }
 }
-/*  update.attach(&update_handler, pend.UPDATE_TIME);
+  update.attach(&update_handler, pend.UPDATE_TIME);
   wait(1);
   pend.set_zeros(); 
     while(1)pend.print_test();
-    
+ */   
 }
-*/
+