Diff: main.cpp

Revision:

23:5238b046119b

Parent:

22:c18f04d1dc49

Child:

24:8a62311f2c5e

--- a/main.cpp	Fri Nov 18 02:47:59 2016 +0000
+++ b/main.cpp	Fri Nov 18 11:55:33 2016 +0000
@@ -4,55 +4,114 @@
 #include "list.h"
 #include <time.h>
 #include "pid.h"
+
+#define TRACK_LENGTH 296
+#define TRACK_MIDDLE TRACK_LENGTH/2
+#define Kp 1
+#define Ki 100
+#define Kd 100
  
 Serial pc(USBTX, USBRX);
 //Use X4 encoding.
 //QEI wheel(p29, p30, NC, 624, QEI::X4_ENCODING);
 //Use X2 encoding by default.
 QEI encoder (PTC16, PTC17, PTB9, 512);
-Motor motor(PTC4, PTD0, PTC3, PTC12, PTB23, 2000);
+Motor motor(PTC4, PTD0, PTC3, PTC12, PTB23, 400);
 Pot pend(PTC10, &encoder,0);
-PID pid = PID(pend.UPDATE_TIME, 4700, -4700, 500, 0, 0); 
+PID pid = PID(pend.UPDATE_TIME, 148, -148, Kp, 0, 0); 
 Ticker update;
 
-void test_distance(){
-    int wait = 1000;
-    int steps = 100;
-    while (wait > 500){
-        while (steps > 10){
-            printf("steps: %i wait: %i \r\n", steps, wait);
-            for( int i = 0; i < 3; i++){
-                encoder.reset();
-                motor.step_clockwise(steps, wait);
-                int pulses_cw = encoder.getPulses();
-                wait_ms(200);
-                int coast_pulses_cw = encoder.getPulses();
-                
-                encoder.reset();
-                motor.step_anticlockwise(steps, wait);
-                int pulses_ccw = encoder.getPulses();
-                wait_ms(200);
-                int coast_pulses_ccw = encoder.getPulses();
-                printf("trial: %i \tclockwise pluses: \t%i  \tanticlockwise pluses: \t%i \r\n", i + 1, pulses_cw, pulses_ccw);
-                printf("\t\tpulses after coast: \t%i \tpulses after coast: \t%i\r\n", coast_pulses_cw, coast_pulses_ccw);
-            }
-            steps -=10;
-            printf("\r\n");
+
+/*
+void update_handler(){
+    pend.update();
+//    pid.testError((float) 180, (float) pend.angle);
+    float output = pid.calculate(180, pend.angle);
+    //printf("output: %f angle: %f\r\n",output , pend.angle);
+    if(output > 0){
+        motor.delay = 3000 - (int) output;
+        if (motor.dir != false) { 
+            wait_us(100);
+        }
+        motor.dir = false;
+    }else{
+        motor.delay = 3000 + (int) output;
+        if (motor.dir != true) {
+            wait_us(100);    
         }
-        
-        wait-=100;
-        steps=100;
-    }       
+        motor.dir = true;
+    }  
+}
+*/
+
+void update_handler2(){
+    pend.update();
+    float output = pid.calculate(180, pend.angle);
+    motor.length = int(5*output);
+    if(output > 0) {
+        if(motor.dir != false) {
+            wait_us(100);
+        }
+        motor.dir = false;
+    }
+    else {
+        if(motor.dir != true) {
+            wait_us(100);  
+        }
+        motor.dir = true; 
+    }
 }
 
 
+int main(){
+    wait(2);
+    pend.set_zeros();
+    motor.initialize(TRACK_MIDDLE);
+    wait(2);
+    update.attach(&update_handler2, pend.UPDATE_TIME);
+    while(motor.steps > 6 && motor.steps < 290){
+        //pend.print_test();
+        motor.run2(true);
+    } 
+    
+/*    motor.step_clockwise(74);  
+    for (int i = 1000; i != 400; i -= 50) {
+        wait_us(100);
+        motor.step_clockwise(148, i);
+        wait_us(100);
+        motor.step_anticlockwise(148, i);   
+    }   
+    
+    while (1) {
+        motor.step_clockwise(149);
+        wait_us(500);
+        motor.step_anticlockwise(149);
+        wait_us(500);
+    }        
+*/
+   
+/* 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);
+    }
 
-void calibrate_pendulum(){
-    encoder.reset();
+   pend.set_zeros();
+    int waits = 700;
     while(1){
-        printf("voltage %%: %f  pulse: %i\r\n", pend.angle_as_voltage(), encoder.getPulses());
+        move_pulses(1000*neg, waits);
+        neg*= -1;
+        if(neg == -1)
+            waits -= 10;
+        printf("Wait time: %i\r\n", waits);
+        pend.print_test();
+        wait(3);
     }
 }
+
 void move_pulses(int pulses, int wait){ // find number of pulses from the encoder going from one end to the other.
     int start = encoder.getPulses();
     if(pulses >= 0){
@@ -89,57 +148,35 @@
     }       
 }
 
-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;
-    }  
+void test_distance(){
+    int wait = 1000;
+    int steps = 100;
+    while (wait > 500){
+        while (steps > 10){
+            printf("steps: %i wait: %i \r\n", steps, wait);
+            for( int i = 0; i < 3; i++){
+                encoder.reset();
+//                motor.step_clockwise(steps, wait);
+                int pulses_cw = encoder.getPulses();
+                wait_ms(200);
+                int coast_pulses_cw = encoder.getPulses();
+                
+                encoder.reset();
+//                motor.step_anticlockwise(steps, wait);
+                int pulses_ccw = encoder.getPulses();
+                wait_ms(200);
+                int coast_pulses_ccw = encoder.getPulses();
+                printf("trial: %i \tclockwise pluses: \t%i  \tanticlockwise pluses: \t%i \r\n", i + 1, pulses_cw, pulses_ccw);
+                printf("\t\tpulses after coast: \t%i \tpulses after coast: \t%i\r\n", coast_pulses_cw, coast_pulses_ccw);
+            }
+            steps -=10;
+            printf("\r\n");
+        }
+        
+        wait-=100;
+        steps=100;
+    }       
 }
-int main(){
-    wait(2);
-    pend.set_zeros();
-    update.attach(&update_handler, pend.UPDATE_TIME);
-    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);
-        neg*= -1;
-        if(neg == -1)
-            waits -= 10;
-        printf("Wait time: %i\r\n", waits);
-        pend.print_test();
-        wait(3);
-    }
-}
-  update.attach(&update_handler, pend.UPDATE_TIME);
-  wait(1);
-  pend.set_zeros(); 
-    while(1)pend.print_test();
  */   
 }
-