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@23:5238b046119b, 2016-11-18 (annotated)

Committer:
nolan21
Date:
Fri Nov 18 11:55:33 2016 +0000
Revision:
23:5238b046119b
Parent:
22:c18f04d1dc49
Child:
24:8a62311f2c5e
almost there boys

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