Rianne Bulthuis / Mbed 2 deprecated cases_motor_2

case motor 2

Dependencies:   Encoder HIDScope MODSERIAL mbed

main.cpp@0:f99a696015a0, 2015-10-22 (annotated)

Committer:
riannebulthuis
Date:
Thu Oct 22 15:04:55 2015 +0000
Revision:
0:f99a696015a0
case motor 2

Who changed what in which revision?

UserRevisionLine numberNew contents of line
riannebulthuis 0:f99a696015a0 1 #include "mbed.h"
riannebulthuis 0:f99a696015a0 2 #include "encoder.h"
riannebulthuis 0:f99a696015a0 3 #include "HIDScope.h"
riannebulthuis 0:f99a696015a0 4 #include "MODSERIAL.h"
riannebulthuis 0:f99a696015a0 5
riannebulthuis 0:f99a696015a0 6 DigitalOut motor2_direction(D7);
riannebulthuis 0:f99a696015a0 7 PwmOut motor2_speed(D6);
riannebulthuis 0:f99a696015a0 8 DigitalIn button_1(PTC6); //counterclockwise
riannebulthuis 0:f99a696015a0 9 DigitalIn button_2(PTA4); //clockwise
riannebulthuis 0:f99a696015a0 10 AnalogIn PotMeter2(A5);
riannebulthuis 0:f99a696015a0 11 Ticker controller;
riannebulthuis 0:f99a696015a0 12 Ticker ticker_regelaar;
riannebulthuis 0:f99a696015a0 13 Ticker Scope;
riannebulthuis 0:f99a696015a0 14 Encoder motor2(D10,D11);
riannebulthuis 0:f99a696015a0 15 HIDScope scope(1);
riannebulthuis 0:f99a696015a0 16 MODSERIAL pc(USBTX, USBRX);
riannebulthuis 0:f99a696015a0 17
riannebulthuis 0:f99a696015a0 18 // Regelaar homeposition
riannebulthuis 0:f99a696015a0 19 #define SAMPLETIME_REGELAAR 0.005
riannebulthuis 0:f99a696015a0 20 volatile bool regelaar_ticker_flag;
riannebulthuis 0:f99a696015a0 21 void setregelaar_ticker_flag(){
riannebulthuis 0:f99a696015a0 22 regelaar_ticker_flag = true;
riannebulthuis 0:f99a696015a0 23 }
riannebulthuis 0:f99a696015a0 24
riannebulthuis 0:f99a696015a0 25 //define states
riannebulthuis 0:f99a696015a0 26 enum state {HOME, MOVE_MOTOR_1, MOVE_MOTOR_2, BACKTOHOMEPOSITION, STOP};
riannebulthuis 0:f99a696015a0 27 uint8_t state = HOME;
riannebulthuis 0:f99a696015a0 28
riannebulthuis 0:f99a696015a0 29 // Berekening van de output shaft resolution. Het aantal counts per 1 graden verandering (PI-controller)
riannebulthuis 0:f99a696015a0 30 const double g = 360; // Aantal graden 1 rotatie
riannebulthuis 0:f99a696015a0 31 const double c = 4200; // Aantal counts 1 rotatie
riannebulthuis 0:f99a696015a0 32 const double q = c/(g);
riannebulthuis 0:f99a696015a0 33
riannebulthuis 0:f99a696015a0 34 //PI-controller constante
riannebulthuis 0:f99a696015a0 35 const double motor2_Kp = 2.0; // Dit is de proportionele gain motor 1
riannebulthuis 0:f99a696015a0 36 const double motor2_Ki = 0.002; // Integrating gain m1.
riannebulthuis 0:f99a696015a0 37 const double motor2_Ts = 0.01; // Time step m1
riannebulthuis 0:f99a696015a0 38 double err_int_m2 = 0 ; // De integrating error op het beginstijdstip m1
riannebulthuis 0:f99a696015a0 39
riannebulthuis 0:f99a696015a0 40 // Reusable P controller
riannebulthuis 0:f99a696015a0 41 double Pc (double error, const double Kp){
riannebulthuis 0:f99a696015a0 42 return motor2_Kp * error;
riannebulthuis 0:f99a696015a0 43 }
riannebulthuis 0:f99a696015a0 44
riannebulthuis 0:f99a696015a0 45 // Measure the error and apply output to the plant
riannebulthuis 0:f99a696015a0 46 void motor2_controlP(){
riannebulthuis 0:f99a696015a0 47 double referenceP2 = PotMeter2.read();
riannebulthuis 0:f99a696015a0 48 double positionP2 = q*motor2.getPosition();
riannebulthuis 0:f99a696015a0 49 double motorP2 = Pc(referenceP2 - positionP2, motor2_Kp);
riannebulthuis 0:f99a696015a0 50 }
riannebulthuis 0:f99a696015a0 51
riannebulthuis 0:f99a696015a0 52 // Reusable PI controller
riannebulthuis 0:f99a696015a0 53 double PI (double error, const double Kp, const double Ki, const double Ts, double &err_int){
riannebulthuis 0:f99a696015a0 54 err_int = err_int * Ts*error; // Dit is de fout die er door de integrator uit wordt gehaald. Deze wordt elke meting aangepast door het &-teken
riannebulthuis 0:f99a696015a0 55 return motor2_Kp*error + motor2_Ki*err_int;
riannebulthuis 0:f99a696015a0 56 } // De totale fout die wordt hersteld met behulp van PI control.
riannebulthuis 0:f99a696015a0 57
riannebulthuis 0:f99a696015a0 58 void motor2_controlPI(){
riannebulthuis 0:f99a696015a0 59 double referencePI2 = PotMeter2.read();
riannebulthuis 0:f99a696015a0 60 double positionPI2 = q*motor2.getPosition();
riannebulthuis 0:f99a696015a0 61 double motorPI2 = PI(referencePI2 - positionPI2, motor2_Kp, motor2_Ki, motor2_Ts, err_int_m2);
riannebulthuis 0:f99a696015a0 62 }
riannebulthuis 0:f99a696015a0 63
riannebulthuis 0:f99a696015a0 64 const int pressed = 0;
riannebulthuis 0:f99a696015a0 65
riannebulthuis 0:f99a696015a0 66 // constantes voor berekening homepositie
riannebulthuis 0:f99a696015a0 67 double H;
riannebulthuis 0:f99a696015a0 68 double P;
riannebulthuis 0:f99a696015a0 69 double D;
riannebulthuis 0:f99a696015a0 70
riannebulthuis 0:f99a696015a0 71
riannebulthuis 0:f99a696015a0 72 void move_motor2()
riannebulthuis 0:f99a696015a0 73 {
riannebulthuis 0:f99a696015a0 74 if (button_1 == pressed){
riannebulthuis 0:f99a696015a0 75 pc.printf("Moving clockwise \n\r");
riannebulthuis 0:f99a696015a0 76 motor2_direction = 1; //clockwise
riannebulthuis 0:f99a696015a0 77 motor2_speed = 0.4;
riannebulthuis 0:f99a696015a0 78 }
riannebulthuis 0:f99a696015a0 79 else if (button_2 == pressed){
riannebulthuis 0:f99a696015a0 80 pc.printf("Moving counterclockwise \n\r");
riannebulthuis 0:f99a696015a0 81 motor2_direction = 0; //counterclockwise
riannebulthuis 0:f99a696015a0 82 motor2_speed = 0.4;
riannebulthuis 0:f99a696015a0 83 }
riannebulthuis 0:f99a696015a0 84 else {
riannebulthuis 0:f99a696015a0 85 pc.printf("Not moving \n\r");
riannebulthuis 0:f99a696015a0 86 motor2_speed = 0;
riannebulthuis 0:f99a696015a0 87 }
riannebulthuis 0:f99a696015a0 88 }
riannebulthuis 0:f99a696015a0 89
riannebulthuis 0:f99a696015a0 90 void movetohome(){
riannebulthuis 0:f99a696015a0 91 P = motor2.getPosition();
riannebulthuis 0:f99a696015a0 92
riannebulthuis 0:f99a696015a0 93 if (P >= -28 && P <= 28){
riannebulthuis 0:f99a696015a0 94 motor2_speed = 0;
riannebulthuis 0:f99a696015a0 95 }
riannebulthuis 0:f99a696015a0 96 else if (P > 24){
riannebulthuis 0:f99a696015a0 97 motor2_direction = 1;
riannebulthuis 0:f99a696015a0 98 motor2_speed = 0.1;
riannebulthuis 0:f99a696015a0 99 }
riannebulthuis 0:f99a696015a0 100 else if (P < -24){
riannebulthuis 0:f99a696015a0 101 motor2_direction = 0;
riannebulthuis 0:f99a696015a0 102 motor2_speed = 0.1;
riannebulthuis 0:f99a696015a0 103 }
riannebulthuis 0:f99a696015a0 104 }
riannebulthuis 0:f99a696015a0 105
riannebulthuis 0:f99a696015a0 106 void HIDScope (){
riannebulthuis 0:f99a696015a0 107 scope.set(0, motor2.getPosition());
riannebulthuis 0:f99a696015a0 108 scope.send ();
riannebulthuis 0:f99a696015a0 109 }
riannebulthuis 0:f99a696015a0 110
riannebulthuis 0:f99a696015a0 111 int main()
riannebulthuis 0:f99a696015a0 112 {
riannebulthuis 0:f99a696015a0 113 while (true) {
riannebulthuis 0:f99a696015a0 114 pc.baud(9600); //pc baud rate van de computer
riannebulthuis 0:f99a696015a0 115 Scope.attach_us(HIDScope, 1e3); //EMG en encoder signaal naar de hidscope sturen
riannebulthuis 0:f99a696015a0 116
riannebulthuis 0:f99a696015a0 117 switch (state) { //zorgt voor het in goede volgorde uitvoeren van de cases
riannebulthuis 0:f99a696015a0 118
riannebulthuis 0:f99a696015a0 119 case HOME: //positie op 0 zetten voor arm 1
riannebulthuis 0:f99a696015a0 120 {
riannebulthuis 0:f99a696015a0 121 pc.printf("home\n\r");
riannebulthuis 0:f99a696015a0 122 H = motor2.getPosition();
riannebulthuis 0:f99a696015a0 123 pc.printf("Home-position is %f\n\r", H);
riannebulthuis 0:f99a696015a0 124 state = MOVE_MOTOR_1;
riannebulthuis 0:f99a696015a0 125 wait(2);
riannebulthuis 0:f99a696015a0 126 break;
riannebulthuis 0:f99a696015a0 127 }
riannebulthuis 0:f99a696015a0 128
riannebulthuis 0:f99a696015a0 129 case MOVE_MOTOR_1: //motor kan cw en ccw bewegen a.d.h.v. buttons
riannebulthuis 0:f99a696015a0 130 {
riannebulthuis 0:f99a696015a0 131 pc.printf("move_motor1\n\r");
riannebulthuis 0:f99a696015a0 132 wait (1);
riannebulthuis 0:f99a696015a0 133 state = MOVE_MOTOR_2;
riannebulthuis 0:f99a696015a0 134 break;
riannebulthuis 0:f99a696015a0 135 }
riannebulthuis 0:f99a696015a0 136
riannebulthuis 0:f99a696015a0 137 case MOVE_MOTOR_2: //motor kan cw en ccw bewegen a.d.h.v. buttons
riannebulthuis 0:f99a696015a0 138 {
riannebulthuis 0:f99a696015a0 139 pc.printf("move_motor\n\r");
riannebulthuis 0:f99a696015a0 140 while (state == MOVE_MOTOR_2){
riannebulthuis 0:f99a696015a0 141 move_motor2();
riannebulthuis 0:f99a696015a0 142 if (button_1 == pressed && button_2 == pressed){
riannebulthuis 0:f99a696015a0 143 motor2_speed = 0;
riannebulthuis 0:f99a696015a0 144 state = BACKTOHOMEPOSITION;
riannebulthuis 0:f99a696015a0 145 }
riannebulthuis 0:f99a696015a0 146 }
riannebulthuis 0:f99a696015a0 147 wait (1);
riannebulthuis 0:f99a696015a0 148 break;
riannebulthuis 0:f99a696015a0 149 }
riannebulthuis 0:f99a696015a0 150
riannebulthuis 0:f99a696015a0 151 case BACKTOHOMEPOSITION: //motor gaat terug naar homeposition
riannebulthuis 0:f99a696015a0 152 {
riannebulthuis 0:f99a696015a0 153 pc.printf("backhomeposition\n\r");
riannebulthuis 0:f99a696015a0 154 wait (1);
riannebulthuis 0:f99a696015a0 155
riannebulthuis 0:f99a696015a0 156 ticker_regelaar.attach(setregelaar_ticker_flag, SAMPLETIME_REGELAAR);
riannebulthuis 0:f99a696015a0 157 while(state == BACKTOHOMEPOSITION){
riannebulthuis 0:f99a696015a0 158 movetohome();
riannebulthuis 0:f99a696015a0 159 while(regelaar_ticker_flag != true);
riannebulthuis 0:f99a696015a0 160 regelaar_ticker_flag = false;
riannebulthuis 0:f99a696015a0 161
riannebulthuis 0:f99a696015a0 162 pc.printf("pulsmotorposition1 %d", motor2.getPosition());
riannebulthuis 0:f99a696015a0 163 pc.printf("referentie %f\n\r", H);
riannebulthuis 0:f99a696015a0 164
riannebulthuis 0:f99a696015a0 165 if (P <=24 && P >= -24){
riannebulthuis 0:f99a696015a0 166 pc.printf("pulsmotorposition1 %d", motor2.getPosition());
riannebulthuis 0:f99a696015a0 167 pc.printf("referentie %f\n\r", H);
riannebulthuis 0:f99a696015a0 168 state = STOP;
riannebulthuis 0:f99a696015a0 169 pc.printf("Laatste positie %f\n\r", motor2.getPosition());
riannebulthuis 0:f99a696015a0 170 break;
riannebulthuis 0:f99a696015a0 171 }
riannebulthuis 0:f99a696015a0 172 }
riannebulthuis 0:f99a696015a0 173 }
riannebulthuis 0:f99a696015a0 174 case STOP:
riannebulthuis 0:f99a696015a0 175 {
riannebulthuis 0:f99a696015a0 176 static int c;
riannebulthuis 0:f99a696015a0 177 while(state == STOP){
riannebulthuis 0:f99a696015a0 178 motor2_speed = 0;
riannebulthuis 0:f99a696015a0 179 if (c++ == 0){
riannebulthuis 0:f99a696015a0 180 pc.printf("einde\n\r");
riannebulthuis 0:f99a696015a0 181 }
riannebulthuis 0:f99a696015a0 182 }
riannebulthuis 0:f99a696015a0 183 break;
riannebulthuis 0:f99a696015a0 184 }
riannebulthuis 0:f99a696015a0 185 }
riannebulthuis 0:f99a696015a0 186 }
riannebulthuis 0:f99a696015a0 187 }