Team repo
Embed:
(wiki syntax)
Show/hide line numbers
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 }
Generated on Sat Jul 16 2022 15:10:49 by
1.7.2