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@24:8a62311f2c5e, 2016-11-28 (annotated)

Committer:
rkolli
Date:
Mon Nov 28 02:49:16 2016 +0000
Revision:
24:8a62311f2c5e
Parent:
23:5238b046119b 
commenting

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"
nolan21 23:5238b046119b 7
nolan21 23:5238b046119b 8 #define TRACK_LENGTH 296
nolan21 23:5238b046119b 9 #define TRACK_MIDDLE TRACK_LENGTH/2
rkolli 24:8a62311f2c5e 10
rkolli 24:8a62311f2c5e 11 #define Kp 1 // Typically the main drive in a control loop, KP reduces a large part of the overall error.
rkolli 24:8a62311f2c5e 12
rkolli 24:8a62311f2c5e 13 #define Ki 100 //Reduces the final error in a system.
rkolli 24:8a62311f2c5e 14 //Summing even a small error over time produces a drive signal large enough to move the system toward a smaller error.
rkolli 24:8a62311f2c5e 15
rkolli 24:8a62311f2c5e 16 #define Kd 100 // Counteracts the KP and KI terms when the output changes quickly. This helps reduce overshoot and ringing.
rkolli 24:8a62311f2c5e 17 //It has no effect on final error.
rkolli 24:8a62311f2c5e 18
dlweakley 4:41bbbaecd322 19
Snay22 5:74fcd196ff96 20 Serial pc(USBTX, USBRX);
Snay22 5:74fcd196ff96 21 //Use X4 encoding.
Snay22 5:74fcd196ff96 22 //QEI wheel(p29, p30, NC, 624, QEI::X4_ENCODING);
Snay22 5:74fcd196ff96 23 //Use X2 encoding by default.
dlweakley 8:2abfdbf5a3b8 24 QEI encoder (PTC16, PTC17, PTB9, 512);
nolan21 23:5238b046119b 25 Motor motor(PTC4, PTD0, PTC3, PTC12, PTB23, 400);
dlweakley 20:8063c82bbb35 26 Pot pend(PTC10, &encoder,0);
nolan21 23:5238b046119b 27 PID pid = PID(pend.UPDATE_TIME, 148, -148, Kp, 0, 0);
dlweakley 20:8063c82bbb35 28 Ticker update;
dlweakley 20:8063c82bbb35 29
nolan21 23:5238b046119b 30
nolan21 23:5238b046119b 31 /*
nolan21 23:5238b046119b 32 void update_handler(){
nolan21 23:5238b046119b 33 pend.update();
nolan21 23:5238b046119b 34 // pid.testError((float) 180, (float) pend.angle);
nolan21 23:5238b046119b 35 float output = pid.calculate(180, pend.angle);
nolan21 23:5238b046119b 36 //printf("output: %f angle: %f\r\n",output , pend.angle);
nolan21 23:5238b046119b 37 if(output > 0){
nolan21 23:5238b046119b 38 motor.delay = 3000 - (int) output;
nolan21 23:5238b046119b 39 if (motor.dir != false) {
nolan21 23:5238b046119b 40 wait_us(100);
nolan21 23:5238b046119b 41 }
nolan21 23:5238b046119b 42 motor.dir = false;
nolan21 23:5238b046119b 43 }else{
nolan21 23:5238b046119b 44 motor.delay = 3000 + (int) output;
nolan21 23:5238b046119b 45 if (motor.dir != true) {
nolan21 23:5238b046119b 46 wait_us(100);
dlweakley 8:2abfdbf5a3b8 47 }
nolan21 23:5238b046119b 48 motor.dir = true;
nolan21 23:5238b046119b 49 }
nolan21 23:5238b046119b 50 }
nolan21 23:5238b046119b 51 */
nolan21 23:5238b046119b 52
nolan21 23:5238b046119b 53 void update_handler2(){
nolan21 23:5238b046119b 54 pend.update();
nolan21 23:5238b046119b 55 float output = pid.calculate(180, pend.angle);
nolan21 23:5238b046119b 56 motor.length = int(5*output);
nolan21 23:5238b046119b 57 if(output > 0) {
nolan21 23:5238b046119b 58 if(motor.dir != false) {
nolan21 23:5238b046119b 59 wait_us(100);
nolan21 23:5238b046119b 60 }
nolan21 23:5238b046119b 61 motor.dir = false;
nolan21 23:5238b046119b 62 }
nolan21 23:5238b046119b 63 else {
nolan21 23:5238b046119b 64 if(motor.dir != true) {
nolan21 23:5238b046119b 65 wait_us(100);
nolan21 23:5238b046119b 66 }
nolan21 23:5238b046119b 67 motor.dir = true;
nolan21 23:5238b046119b 68 }
dlweakley 8:2abfdbf5a3b8 69 }
dlweakley 6:8d2171811f14 70
enderceylan 13:ab5a6ab6c492 71
nolan21 23:5238b046119b 72 int main(){
nolan21 23:5238b046119b 73 wait(2);
nolan21 23:5238b046119b 74 pend.set_zeros();
nolan21 23:5238b046119b 75 motor.initialize(TRACK_MIDDLE);
nolan21 23:5238b046119b 76 wait(2);
nolan21 23:5238b046119b 77 update.attach(&update_handler2, pend.UPDATE_TIME);
nolan21 23:5238b046119b 78 while(motor.steps > 6 && motor.steps < 290){
nolan21 23:5238b046119b 79 //pend.print_test();
nolan21 23:5238b046119b 80 motor.run2(true);
nolan21 23:5238b046119b 81 }
nolan21 23:5238b046119b 82
nolan21 23:5238b046119b 83 /* motor.step_clockwise(74);
nolan21 23:5238b046119b 84 for (int i = 1000; i != 400; i -= 50) {
nolan21 23:5238b046119b 85 wait_us(100);
nolan21 23:5238b046119b 86 motor.step_clockwise(148, i);
nolan21 23:5238b046119b 87 wait_us(100);
nolan21 23:5238b046119b 88 motor.step_anticlockwise(148, i);
nolan21 23:5238b046119b 89 }
nolan21 23:5238b046119b 90
nolan21 23:5238b046119b 91 while (1) {
nolan21 23:5238b046119b 92 motor.step_clockwise(149);
nolan21 23:5238b046119b 93 wait_us(500);
nolan21 23:5238b046119b 94 motor.step_anticlockwise(149);
nolan21 23:5238b046119b 95 wait_us(500);
nolan21 23:5238b046119b 96 }
nolan21 23:5238b046119b 97 */
nolan21 23:5238b046119b 98
nolan21 23:5238b046119b 99 /* while(1){
nolan21 23:5238b046119b 100
nolan21 23:5238b046119b 101 //printf("%f\r\n",pend.angle_as_voltage());
nolan21 23:5238b046119b 102 //pend.print_test();
nolan21 23:5238b046119b 103 if(pend.angle>2) motor.anticlockwise(1000);
nolan21 23:5238b046119b 104 else if( (pend.angle>=-2) && (pend.angle<=2))wait_ms(1);
nolan21 23:5238b046119b 105 else motor.clockwise(1000);
nolan21 23:5238b046119b 106 }
dlweakley 8:2abfdbf5a3b8 107
nolan21 23:5238b046119b 108 pend.set_zeros();
nolan21 23:5238b046119b 109 int waits = 700;
dlweakley 11:ed9539245ea0 110 while(1){
nolan21 23:5238b046119b 111 move_pulses(1000*neg, waits);
nolan21 23:5238b046119b 112 neg*= -1;
nolan21 23:5238b046119b 113 if(neg == -1)
nolan21 23:5238b046119b 114 waits -= 10;
nolan21 23:5238b046119b 115 printf("Wait time: %i\r\n", waits);
nolan21 23:5238b046119b 116 pend.print_test();
nolan21 23:5238b046119b 117 wait(3);
dlweakley 11:ed9539245ea0 118 }
dlweakley 11:ed9539245ea0 119 }
nolan21 23:5238b046119b 120
Snay22 19:0e9bf6d61d0d 121 void move_pulses(int pulses, int wait){ // find number of pulses from the encoder going from one end to the other.
dlweakley 11:ed9539245ea0 122 int start = encoder.getPulses();
dlweakley 20:8063c82bbb35 123 if(pulses >= 0){
dlweakley 11:ed9539245ea0 124 while(encoder.getPulses() < start + pulses){
Snay22 21:eac29cf3f061 125 motor.clockwise(wait);
dlweakley 11:ed9539245ea0 126 }
dlweakley 11:ed9539245ea0 127 }else{
dlweakley 20:8063c82bbb35 128 while(encoder.getPulses() > start + pulses){
Snay22 21:eac29cf3f061 129 motor.anticlockwise(wait);
dlweakley 20:8063c82bbb35 130 }
dlweakley 20:8063c82bbb35 131 }
Snay22 18:5a1c351094d2 132 }
dlweakley 22:c18f04d1dc49 133 void test_speed(){
dlweakley 22:c18f04d1dc49 134 int speed = 1000;
dlweakley 22:c18f04d1dc49 135 int decrement = 10;
dlweakley 22:c18f04d1dc49 136
dlweakley 22:c18f04d1dc49 137 List velocities_cw = List(1000);
dlweakley 22:c18f04d1dc49 138 List velocities_ccw = List(1000);
dlweakley 22:c18f04d1dc49 139 printf("hi");
dlweakley 22:c18f04d1dc49 140 while(speed > 400){
dlweakley 22:c18f04d1dc49 141 for(int i =0; i<100; i++){
dlweakley 22:c18f04d1dc49 142 motor.clockwise(speed);
dlweakley 22:c18f04d1dc49 143 velocities_cw.add(pend.velocity);
dlweakley 22:c18f04d1dc49 144 }
dlweakley 22:c18f04d1dc49 145 wait(1);
dlweakley 22:c18f04d1dc49 146 for(int i =0; i<100; i++){
dlweakley 22:c18f04d1dc49 147 motor.anticlockwise(speed);
dlweakley 22:c18f04d1dc49 148 velocities_ccw.add(pend.velocity);
dlweakley 22:c18f04d1dc49 149 }
dlweakley 22:c18f04d1dc49 150 wait(1);
dlweakley 22:c18f04d1dc49 151 printf("clockwise velocity peak: %f avg: %f anticlockwise velocity peak: %f avg: %f \r\n"
dlweakley 22:c18f04d1dc49 152 ,velocities_cw.min(), velocities_cw.average(), velocities_ccw.max(), velocities_ccw.average());
dlweakley 22:c18f04d1dc49 153 speed -= decrement;
dlweakley 22:c18f04d1dc49 154 }
dlweakley 22:c18f04d1dc49 155 }
Snay22 18:5a1c351094d2 156
nolan21 23:5238b046119b 157 void test_distance(){
nolan21 23:5238b046119b 158 int wait = 1000;
nolan21 23:5238b046119b 159 int steps = 100;
nolan21 23:5238b046119b 160 while (wait > 500){
nolan21 23:5238b046119b 161 while (steps > 10){
nolan21 23:5238b046119b 162 printf("steps: %i wait: %i \r\n", steps, wait);
nolan21 23:5238b046119b 163 for( int i = 0; i < 3; i++){
nolan21 23:5238b046119b 164 encoder.reset();
nolan21 23:5238b046119b 165 // motor.step_clockwise(steps, wait);
nolan21 23:5238b046119b 166 int pulses_cw = encoder.getPulses();
nolan21 23:5238b046119b 167 wait_ms(200);
nolan21 23:5238b046119b 168 int coast_pulses_cw = encoder.getPulses();
nolan21 23:5238b046119b 169
nolan21 23:5238b046119b 170 encoder.reset();
nolan21 23:5238b046119b 171 // motor.step_anticlockwise(steps, wait);
nolan21 23:5238b046119b 172 int pulses_ccw = encoder.getPulses();
nolan21 23:5238b046119b 173 wait_ms(200);
nolan21 23:5238b046119b 174 int coast_pulses_ccw = encoder.getPulses();
nolan21 23:5238b046119b 175 printf("trial: %i \tclockwise pluses: \t%i \tanticlockwise pluses: \t%i \r\n", i + 1, pulses_cw, pulses_ccw);
nolan21 23:5238b046119b 176 printf("\t\tpulses after coast: \t%i \tpulses after coast: \t%i\r\n", coast_pulses_cw, coast_pulses_ccw);
nolan21 23:5238b046119b 177 }
nolan21 23:5238b046119b 178 steps -=10;
nolan21 23:5238b046119b 179 printf("\r\n");
nolan21 23:5238b046119b 180 }
nolan21 23:5238b046119b 181
nolan21 23:5238b046119b 182 wait-=100;
nolan21 23:5238b046119b 183 steps=100;
nolan21 23:5238b046119b 184 }
dlweakley 20:8063c82bbb35 185 }
dlweakley 22:c18f04d1dc49 186
dlweakley 22:c18f04d1dc49 187 */
dlweakley 8:2abfdbf5a3b8 188 }