Team repo

Dependencies:   mbed QEI

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers main.cpp Source File

main.cpp

00001 #include "QEI.h"
00002 #include "Motor.h"
00003 #include "pot.h"
00004 #include "list.h"
00005 #include <time.h>
00006 #include "pid.h"
00007 
00008 #define TRACK_LENGTH 296
00009 #define TRACK_MIDDLE TRACK_LENGTH/2
00010 
00011 #define Kp 1    // Typically the main drive in a control loop, KP reduces a large part of the overall error.
00012 
00013 #define Ki 100 //Reduces the final error in a system. 
00014                //Summing even a small error over time produces a drive signal large enough to move the system toward a smaller error.
00015                
00016 #define Kd 100 // Counteracts the KP and KI terms when the output changes quickly. This helps reduce overshoot and ringing. 
00017                //It has no effect on final error.
00018 
00019  
00020 Serial pc(USBTX, USBRX);
00021 //Use X4 encoding.
00022 //QEI wheel(p29, p30, NC, 624, QEI::X4_ENCODING);
00023 //Use X2 encoding by default.
00024 QEI encoder (PTC16, PTC17, PTB9, 512);
00025 Motor motor(PTC4, PTD0, PTC3, PTC12, PTB23, 400);
00026 Pot pend(PTC10, &encoder,0);
00027 PID pid = PID(pend.UPDATE_TIME, 148, -148, Kp, 0, 0); 
00028 Ticker update;
00029 
00030 
00031 /*
00032 void update_handler(){
00033     pend.update();
00034 //    pid.testError((float) 180, (float) pend.angle);
00035     float output = pid.calculate(180, pend.angle);
00036     //printf("output: %f angle: %f\r\n",output , pend.angle);
00037     if(output > 0){
00038         motor.delay = 3000 - (int) output;
00039         if (motor.dir != false) { 
00040             wait_us(100);
00041         }
00042         motor.dir = false;
00043     }else{
00044         motor.delay = 3000 + (int) output;
00045         if (motor.dir != true) {
00046             wait_us(100);    
00047         }
00048         motor.dir = true;
00049     }  
00050 }
00051 */
00052 
00053 void update_handler2(){
00054     pend.update();
00055     float output = pid.calculate(180, pend.angle);
00056     motor.length = int(5*output);
00057     if(output > 0) {
00058         if(motor.dir != false) {
00059             wait_us(100);
00060         }
00061         motor.dir = false;
00062     }
00063     else {
00064         if(motor.dir != true) {
00065             wait_us(100);  
00066         }
00067         motor.dir = true; 
00068     }
00069 }
00070 
00071 
00072 int main(){
00073     wait(2);
00074     pend.set_zeros();
00075     motor.initialize(TRACK_MIDDLE);
00076     wait(2);
00077     update.attach(&update_handler2, pend.UPDATE_TIME);
00078     while(motor.steps > 6 && motor.steps < 290){
00079         //pend.print_test();
00080         motor.run2(true);
00081     } 
00082     
00083 /*    motor.step_clockwise(74);  
00084     for (int i = 1000; i != 400; i -= 50) {
00085         wait_us(100);
00086         motor.step_clockwise(148, i);
00087         wait_us(100);
00088         motor.step_anticlockwise(148, i);   
00089     }   
00090     
00091     while (1) {
00092         motor.step_clockwise(149);
00093         wait_us(500);
00094         motor.step_anticlockwise(149);
00095         wait_us(500);
00096     }        
00097 */
00098    
00099 /* while(1){
00100         
00101         //printf("%f\r\n",pend.angle_as_voltage());
00102         //pend.print_test();
00103         if(pend.angle>2) motor.anticlockwise(1000);
00104         else if( (pend.angle>=-2) && (pend.angle<=2))wait_ms(1);
00105         else motor.clockwise(1000);
00106     }
00107 
00108    pend.set_zeros();
00109     int waits = 700;
00110     while(1){
00111         move_pulses(1000*neg, waits);
00112         neg*= -1;
00113         if(neg == -1)
00114             waits -= 10;
00115         printf("Wait time: %i\r\n", waits);
00116         pend.print_test();
00117         wait(3);
00118     }
00119 }
00120 
00121 void move_pulses(int pulses, int wait){ // find number of pulses from the encoder going from one end to the other.
00122     int start = encoder.getPulses();
00123     if(pulses >= 0){
00124         while(encoder.getPulses() < start + pulses){
00125             motor.clockwise(wait);    
00126         } 
00127     }else{
00128         while(encoder.getPulses() > start + pulses){
00129             motor.anticlockwise(wait);
00130         }
00131     } 
00132 }
00133 void test_speed(){
00134     int speed = 1000;
00135     int decrement = 10;
00136  
00137     List velocities_cw = List(1000);
00138     List velocities_ccw = List(1000);
00139     printf("hi");
00140     while(speed > 400){
00141         for(int i =0; i<100; i++){
00142             motor.clockwise(speed);
00143             velocities_cw.add(pend.velocity);
00144         }
00145         wait(1);
00146         for(int i =0; i<100; i++){
00147             motor.anticlockwise(speed);
00148             velocities_ccw.add(pend.velocity);
00149         }
00150         wait(1);
00151         printf("clockwise velocity peak: %f avg: %f anticlockwise velocity peak: %f avg: %f \r\n"
00152          ,velocities_cw.min(), velocities_cw.average(),  velocities_ccw.max(), velocities_ccw.average());
00153         speed -= decrement;
00154     }       
00155 }
00156 
00157 void test_distance(){
00158     int wait = 1000;
00159     int steps = 100;
00160     while (wait > 500){
00161         while (steps > 10){
00162             printf("steps: %i wait: %i \r\n", steps, wait);
00163             for( int i = 0; i < 3; i++){
00164                 encoder.reset();
00165 //                motor.step_clockwise(steps, wait);
00166                 int pulses_cw = encoder.getPulses();
00167                 wait_ms(200);
00168                 int coast_pulses_cw = encoder.getPulses();
00169                 
00170                 encoder.reset();
00171 //                motor.step_anticlockwise(steps, wait);
00172                 int pulses_ccw = encoder.getPulses();
00173                 wait_ms(200);
00174                 int coast_pulses_ccw = encoder.getPulses();
00175                 printf("trial: %i \tclockwise pluses: \t%i  \tanticlockwise pluses: \t%i \r\n", i + 1, pulses_cw, pulses_ccw);
00176                 printf("\t\tpulses after coast: \t%i \tpulses after coast: \t%i\r\n", coast_pulses_cw, coast_pulses_ccw);
00177             }
00178             steps -=10;
00179             printf("\r\n");
00180         }
00181         
00182         wait-=100;
00183         steps=100;
00184     }       
00185 }
00186 
00187  */   
00188 }