Inverted Pendulum / Mbed 2 deprecated IP-Interface

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed QEI

main.cpp@22:c18f04d1dc49, 2016-11-18 (annotated)

Committer:
dlweakley
Date:
Fri Nov 18 02:47:59 2016 +0000
Revision:
22:c18f04d1dc49
Parent:
21:eac29cf3f061
Child:
23:5238b046119b
PID attempt

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Snay22 5:74fcd196ff96 1 #include "QEI.h"
dlweakley 6:8d2171811f14 2 #include "Motor.h"
dlweakley 8:2abfdbf5a3b8 3 #include "pot.h"
dlweakley 22:c18f04d1dc49 4 #include "list.h"
enderceylan 13:ab5a6ab6c492 5 #include <time.h>
dlweakley 22:c18f04d1dc49 6 #include "pid.h"
dlweakley 4:41bbbaecd322 7
Snay22 5:74fcd196ff96 8 Serial pc(USBTX, USBRX);
Snay22 5:74fcd196ff96 9 //Use X4 encoding.
Snay22 5:74fcd196ff96 10 //QEI wheel(p29, p30, NC, 624, QEI::X4_ENCODING);
Snay22 5:74fcd196ff96 11 //Use X2 encoding by default.
dlweakley 8:2abfdbf5a3b8 12 QEI encoder (PTC16, PTC17, PTB9, 512);
dlweakley 11:ed9539245ea0 13 Motor motor(PTC4, PTD0, PTC3, PTC12, PTB23, 2000);
dlweakley 20:8063c82bbb35 14 Pot pend(PTC10, &encoder,0);
dlweakley 22:c18f04d1dc49 15 PID pid = PID(pend.UPDATE_TIME, 4700, -4700, 500, 0, 0);
dlweakley 20:8063c82bbb35 16 Ticker update;
dlweakley 20:8063c82bbb35 17
dlweakley 8:2abfdbf5a3b8 18 void test_distance(){
dlweakley 8:2abfdbf5a3b8 19 int wait = 1000;
dlweakley 8:2abfdbf5a3b8 20 int steps = 100;
dlweakley 8:2abfdbf5a3b8 21 while (wait > 500){
dlweakley 8:2abfdbf5a3b8 22 while (steps > 10){
dlweakley 8:2abfdbf5a3b8 23 printf("steps: %i wait: %i \r\n", steps, wait);
dlweakley 8:2abfdbf5a3b8 24 for( int i = 0; i < 3; i++){
dlweakley 8:2abfdbf5a3b8 25 encoder.reset();
dlweakley 8:2abfdbf5a3b8 26 motor.step_clockwise(steps, wait);
dlweakley 8:2abfdbf5a3b8 27 int pulses_cw = encoder.getPulses();
dlweakley 8:2abfdbf5a3b8 28 wait_ms(200);
dlweakley 8:2abfdbf5a3b8 29 int coast_pulses_cw = encoder.getPulses();
dlweakley 8:2abfdbf5a3b8 30
dlweakley 8:2abfdbf5a3b8 31 encoder.reset();
dlweakley 8:2abfdbf5a3b8 32 motor.step_anticlockwise(steps, wait);
dlweakley 8:2abfdbf5a3b8 33 int pulses_ccw = encoder.getPulses();
dlweakley 8:2abfdbf5a3b8 34 wait_ms(200);
dlweakley 8:2abfdbf5a3b8 35 int coast_pulses_ccw = encoder.getPulses();
dlweakley 8:2abfdbf5a3b8 36 printf("trial: %i \tclockwise pluses: \t%i \tanticlockwise pluses: \t%i \r\n", i + 1, pulses_cw, pulses_ccw);
dlweakley 8:2abfdbf5a3b8 37 printf("\t\tpulses after coast: \t%i \tpulses after coast: \t%i\r\n", coast_pulses_cw, coast_pulses_ccw);
dlweakley 8:2abfdbf5a3b8 38 }
dlweakley 8:2abfdbf5a3b8 39 steps -=10;
dlweakley 8:2abfdbf5a3b8 40 printf("\r\n");
dlweakley 8:2abfdbf5a3b8 41 }
dlweakley 8:2abfdbf5a3b8 42
dlweakley 8:2abfdbf5a3b8 43 wait-=100;
dlweakley 8:2abfdbf5a3b8 44 steps=100;
dlweakley 8:2abfdbf5a3b8 45 }
dlweakley 8:2abfdbf5a3b8 46 }
dlweakley 6:8d2171811f14 47
enderceylan 13:ab5a6ab6c492 48
dlweakley 8:2abfdbf5a3b8 49
dlweakley 11:ed9539245ea0 50 void calibrate_pendulum(){
dlweakley 11:ed9539245ea0 51 encoder.reset();
dlweakley 11:ed9539245ea0 52 while(1){
dlweakley 22:c18f04d1dc49 53 printf("voltage %%: %f pulse: %i\r\n", pend.angle_as_voltage(), encoder.getPulses());
dlweakley 11:ed9539245ea0 54 }
dlweakley 11:ed9539245ea0 55 }
Snay22 19:0e9bf6d61d0d 56 void move_pulses(int pulses, int wait){ // find number of pulses from the encoder going from one end to the other.
dlweakley 11:ed9539245ea0 57 int start = encoder.getPulses();
dlweakley 20:8063c82bbb35 58 if(pulses >= 0){
dlweakley 11:ed9539245ea0 59 while(encoder.getPulses() < start + pulses){
Snay22 21:eac29cf3f061 60 motor.clockwise(wait);
dlweakley 11:ed9539245ea0 61 }
dlweakley 11:ed9539245ea0 62 }else{
dlweakley 20:8063c82bbb35 63 while(encoder.getPulses() > start + pulses){
Snay22 21:eac29cf3f061 64 motor.anticlockwise(wait);
dlweakley 20:8063c82bbb35 65 }
dlweakley 20:8063c82bbb35 66 }
Snay22 18:5a1c351094d2 67 }
dlweakley 22:c18f04d1dc49 68 void test_speed(){
dlweakley 22:c18f04d1dc49 69 int speed = 1000;
dlweakley 22:c18f04d1dc49 70 int decrement = 10;
dlweakley 22:c18f04d1dc49 71
dlweakley 22:c18f04d1dc49 72 List velocities_cw = List(1000);
dlweakley 22:c18f04d1dc49 73 List velocities_ccw = List(1000);
dlweakley 22:c18f04d1dc49 74 printf("hi");
dlweakley 22:c18f04d1dc49 75 while(speed > 400){
dlweakley 22:c18f04d1dc49 76 for(int i =0; i<100; i++){
dlweakley 22:c18f04d1dc49 77 motor.clockwise(speed);
dlweakley 22:c18f04d1dc49 78 velocities_cw.add(pend.velocity);
dlweakley 22:c18f04d1dc49 79 }
dlweakley 22:c18f04d1dc49 80 wait(1);
dlweakley 22:c18f04d1dc49 81 for(int i =0; i<100; i++){
dlweakley 22:c18f04d1dc49 82 motor.anticlockwise(speed);
dlweakley 22:c18f04d1dc49 83 velocities_ccw.add(pend.velocity);
dlweakley 22:c18f04d1dc49 84 }
dlweakley 22:c18f04d1dc49 85 wait(1);
dlweakley 22:c18f04d1dc49 86 printf("clockwise velocity peak: %f avg: %f anticlockwise velocity peak: %f avg: %f \r\n"
dlweakley 22:c18f04d1dc49 87 ,velocities_cw.min(), velocities_cw.average(), velocities_ccw.max(), velocities_ccw.average());
dlweakley 22:c18f04d1dc49 88 speed -= decrement;
dlweakley 22:c18f04d1dc49 89 }
dlweakley 22:c18f04d1dc49 90 }
Snay22 18:5a1c351094d2 91
dlweakley 20:8063c82bbb35 92 void update_handler(){
dlweakley 20:8063c82bbb35 93 pend.update();
dlweakley 22:c18f04d1dc49 94 double output = pid.calculate(180, pend.angle);
dlweakley 22:c18f04d1dc49 95 //printf("output: %f angle: %f\r\n",output , pend.angle);
dlweakley 22:c18f04d1dc49 96 if(output > 0){
dlweakley 22:c18f04d1dc49 97 motor.wait = 5000 - (int) output;
dlweakley 22:c18f04d1dc49 98 motor.dir = false;
dlweakley 22:c18f04d1dc49 99 }else{
dlweakley 22:c18f04d1dc49 100 motor.wait = 5000 + (int) output;
dlweakley 22:c18f04d1dc49 101 motor.dir = true;
dlweakley 22:c18f04d1dc49 102 }
dlweakley 20:8063c82bbb35 103 }
Snay22 21:eac29cf3f061 104 int main(){
dlweakley 22:c18f04d1dc49 105 wait(2);
dlweakley 22:c18f04d1dc49 106 pend.set_zeros();
Snay22 21:eac29cf3f061 107 update.attach(&update_handler, pend.UPDATE_TIME);
dlweakley 22:c18f04d1dc49 108 while(1){
dlweakley 22:c18f04d1dc49 109 //pend.print_test();
dlweakley 22:c18f04d1dc49 110 motor.run(true);
dlweakley 22:c18f04d1dc49 111 //calibrate_pendulum();
dlweakley 22:c18f04d1dc49 112 }
dlweakley 22:c18f04d1dc49 113
dlweakley 22:c18f04d1dc49 114
dlweakley 22:c18f04d1dc49 115
dlweakley 22:c18f04d1dc49 116
dlweakley 22:c18f04d1dc49 117
dlweakley 22:c18f04d1dc49 118 /* while(1){
dlweakley 22:c18f04d1dc49 119
dlweakley 22:c18f04d1dc49 120 //printf("%f\r\n",pend.angle_as_voltage());
dlweakley 22:c18f04d1dc49 121 //pend.print_test();
dlweakley 22:c18f04d1dc49 122 if(pend.angle>2) motor.anticlockwise(1000);
dlweakley 22:c18f04d1dc49 123 else if( (pend.angle>=-2) && (pend.angle<=2))wait_ms(1);
dlweakley 22:c18f04d1dc49 124 else motor.clockwise(1000);
dlweakley 22:c18f04d1dc49 125 }
dlweakley 22:c18f04d1dc49 126
dlweakley 22:c18f04d1dc49 127 pend.set_zeros();
Snay22 21:eac29cf3f061 128 int waits = 700;
Snay22 21:eac29cf3f061 129 while(1){
Snay22 21:eac29cf3f061 130 move_pulses(1000*neg, waits);
Snay22 21:eac29cf3f061 131 neg*= -1;
Snay22 21:eac29cf3f061 132 if(neg == -1)
Snay22 21:eac29cf3f061 133 waits -= 10;
Snay22 21:eac29cf3f061 134 printf("Wait time: %i\r\n", waits);
Snay22 21:eac29cf3f061 135 pend.print_test();
Snay22 21:eac29cf3f061 136 wait(3);
Snay22 21:eac29cf3f061 137 }
Snay22 21:eac29cf3f061 138 }
dlweakley 22:c18f04d1dc49 139 update.attach(&update_handler, pend.UPDATE_TIME);
dlweakley 20:8063c82bbb35 140 wait(1);
dlweakley 20:8063c82bbb35 141 pend.set_zeros();
dlweakley 20:8063c82bbb35 142 while(1)pend.print_test();
dlweakley 22:c18f04d1dc49 143 */
dlweakley 8:2abfdbf5a3b8 144 }
dlweakley 22:c18f04d1dc49 145