Rianne Bulthuis
case motor 2
Dependencies: Encoder HIDScope MODSERIAL mbed
Revision 0:f99a696015a0, committed 2015-10-22
- Comitter:
- riannebulthuis
- Date:
- Thu Oct 22 15:04:55 2015 +0000
- Commit message:
- case motor 2
Changed in this revision
diff -r 000000000000 -r f99a696015a0 Encoder.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Encoder.lib Thu Oct 22 15:04:55 2015 +0000 @@ -0,0 +1,1 @@ +https://developer.mbed.org/users/vsluiter/code/Encoder/#18b000b443af
diff -r 000000000000 -r f99a696015a0 HIDScope.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/HIDScope.lib Thu Oct 22 15:04:55 2015 +0000 @@ -0,0 +1,1 @@ +https://developer.mbed.org/users/tomlankhorst/code/HIDScope/#5020a2c0934b
diff -r 000000000000 -r f99a696015a0 MODSERIAL.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/MODSERIAL.lib Thu Oct 22 15:04:55 2015 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/Sissors/code/MODSERIAL/#6ffa97119f4f
diff -r 000000000000 -r f99a696015a0 main.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Thu Oct 22 15:04:55 2015 +0000
@@ -0,0 +1,187 @@
+#include "mbed.h"
+#include "encoder.h"
+#include "HIDScope.h"
+#include "MODSERIAL.h"
+
+DigitalOut motor2_direction(D7);
+PwmOut motor2_speed(D6);
+DigitalIn button_1(PTC6); //counterclockwise
+DigitalIn button_2(PTA4); //clockwise
+AnalogIn PotMeter2(A5);
+Ticker controller;
+Ticker ticker_regelaar;
+Ticker Scope;
+Encoder motor2(D10,D11);
+HIDScope scope(1);
+MODSERIAL pc(USBTX, USBRX);
+
+// Regelaar homeposition
+#define SAMPLETIME_REGELAAR 0.005
+volatile bool regelaar_ticker_flag;
+void setregelaar_ticker_flag(){
+ regelaar_ticker_flag = true;
+}
+
+//define states
+enum state {HOME, MOVE_MOTOR_1, MOVE_MOTOR_2, BACKTOHOMEPOSITION, STOP};
+uint8_t state = HOME;
+
+// Berekening van de output shaft resolution. Het aantal counts per 1 graden verandering (PI-controller)
+const double g = 360; // Aantal graden 1 rotatie
+const double c = 4200; // Aantal counts 1 rotatie
+const double q = c/(g);
+
+//PI-controller constante
+const double motor2_Kp = 2.0; // Dit is de proportionele gain motor 1
+const double motor2_Ki = 0.002; // Integrating gain m1.
+const double motor2_Ts = 0.01; // Time step m1
+double err_int_m2 = 0 ; // De integrating error op het beginstijdstip m1
+
+// Reusable P controller
+double Pc (double error, const double Kp){
+ return motor2_Kp * error;
+}
+
+// Measure the error and apply output to the plant
+void motor2_controlP(){
+ double referenceP2 = PotMeter2.read();
+ double positionP2 = q*motor2.getPosition();
+ double motorP2 = Pc(referenceP2 - positionP2, motor2_Kp);
+}
+
+// Reusable PI controller
+double PI (double error, const double Kp, const double Ki, const double Ts, double &err_int){
+ 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
+ return motor2_Kp*error + motor2_Ki*err_int;
+} // De totale fout die wordt hersteld met behulp van PI control.
+
+void motor2_controlPI(){
+ double referencePI2 = PotMeter2.read();
+ double positionPI2 = q*motor2.getPosition();
+ double motorPI2 = PI(referencePI2 - positionPI2, motor2_Kp, motor2_Ki, motor2_Ts, err_int_m2);
+}
+
+const int pressed = 0;
+
+// constantes voor berekening homepositie
+double H;
+double P;
+double D;
+
+
+void move_motor2()
+{
+ if (button_1 == pressed){
+ pc.printf("Moving clockwise \n\r");
+ motor2_direction = 1; //clockwise
+ motor2_speed = 0.4;
+ }
+ else if (button_2 == pressed){
+ pc.printf("Moving counterclockwise \n\r");
+ motor2_direction = 0; //counterclockwise
+ motor2_speed = 0.4;
+ }
+ else {
+ pc.printf("Not moving \n\r");
+ motor2_speed = 0;
+ }
+}
+
+void movetohome(){
+ P = motor2.getPosition();
+
+ if (P >= -28 && P <= 28){
+ motor2_speed = 0;
+ }
+ else if (P > 24){
+ motor2_direction = 1;
+ motor2_speed = 0.1;
+ }
+ else if (P < -24){
+ motor2_direction = 0;
+ motor2_speed = 0.1;
+ }
+}
+
+void HIDScope (){
+ scope.set(0, motor2.getPosition());
+ scope.send ();
+ }
+
+int main()
+{
+ while (true) {
+ pc.baud(9600); //pc baud rate van de computer
+ Scope.attach_us(HIDScope, 1e3); //EMG en encoder signaal naar de hidscope sturen
+
+ switch (state) { //zorgt voor het in goede volgorde uitvoeren van de cases
+
+ case HOME: //positie op 0 zetten voor arm 1
+ {
+ pc.printf("home\n\r");
+ H = motor2.getPosition();
+ pc.printf("Home-position is %f\n\r", H);
+ state = MOVE_MOTOR_1;
+ wait(2);
+ break;
+ }
+
+ case MOVE_MOTOR_1: //motor kan cw en ccw bewegen a.d.h.v. buttons
+ {
+ pc.printf("move_motor1\n\r");
+ wait (1);
+ state = MOVE_MOTOR_2;
+ break;
+ }
+
+ case MOVE_MOTOR_2: //motor kan cw en ccw bewegen a.d.h.v. buttons
+ {
+ pc.printf("move_motor\n\r");
+ while (state == MOVE_MOTOR_2){
+ move_motor2();
+ if (button_1 == pressed && button_2 == pressed){
+ motor2_speed = 0;
+ state = BACKTOHOMEPOSITION;
+ }
+ }
+ wait (1);
+ break;
+ }
+
+ case BACKTOHOMEPOSITION: //motor gaat terug naar homeposition
+ {
+ pc.printf("backhomeposition\n\r");
+ wait (1);
+
+ ticker_regelaar.attach(setregelaar_ticker_flag, SAMPLETIME_REGELAAR);
+ while(state == BACKTOHOMEPOSITION){
+ movetohome();
+ while(regelaar_ticker_flag != true);
+ regelaar_ticker_flag = false;
+
+ pc.printf("pulsmotorposition1 %d", motor2.getPosition());
+ pc.printf("referentie %f\n\r", H);
+
+ if (P <=24 && P >= -24){
+ pc.printf("pulsmotorposition1 %d", motor2.getPosition());
+ pc.printf("referentie %f\n\r", H);
+ state = STOP;
+ pc.printf("Laatste positie %f\n\r", motor2.getPosition());
+ break;
+ }
+ }
+ }
+ case STOP:
+ {
+ static int c;
+ while(state == STOP){
+ motor2_speed = 0;
+ if (c++ == 0){
+ pc.printf("einde\n\r");
+ }
+ }
+ break;
+ }
+}
+}
+}
\ No newline at end of file
diff -r 000000000000 -r f99a696015a0 mbed.bld --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Thu Oct 22 15:04:55 2015 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/34e6b704fe68 \ No newline at end of file