Arun Raveenthiran / Mbed 2 deprecated Cases_homepos_picontrol_EMG

hoi

Dependencies:   Encoder HIDScope MODSERIAL mbed

Fork of home_position_1 by Rianne Bulthuis

main.cpp@2:866a8a9f2b93, 2015-10-20 (annotated)

Committer:
riannebulthuis
Date:
Tue Oct 20 12:38:31 2015 +0000
Revision:
2:866a8a9f2b93
Parent:
1:7d5e6bc2b314
Child:
3:5f59cbe53d7d
cases werken. Aanpassen backtohomeposition;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
arunr 0:65ab9f79a4cc 1 #include "mbed.h"
arunr 0:65ab9f79a4cc 2 #include "encoder.h"
arunr 0:65ab9f79a4cc 3 #include "HIDScope.h"
riannebulthuis 1:7d5e6bc2b314 4 #include "QEI.h"
riannebulthuis 1:7d5e6bc2b314 5 #include "MODSERIAL.h"
arunr 0:65ab9f79a4cc 6
arunr 0:65ab9f79a4cc 7 DigitalOut motor1_direction(D4);
arunr 0:65ab9f79a4cc 8 PwmOut motor1_speed(D5);
arunr 0:65ab9f79a4cc 9 PwmOut led(D9);
arunr 0:65ab9f79a4cc 10 DigitalIn button_1(PTC6); //counterclockwise
arunr 0:65ab9f79a4cc 11 DigitalIn button_2(PTA4); //clockwise
arunr 0:65ab9f79a4cc 12 Encoder motor1(D12,D13);
arunr 0:65ab9f79a4cc 13 HIDScope scope(1);
arunr 0:65ab9f79a4cc 14
riannebulthuis 2:866a8a9f2b93 15 MODSERIAL pc(USBTX, USBRX);
riannebulthuis 2:866a8a9f2b93 16 volatile bool regelaar_ticker_flag;
riannebulthuis 2:866a8a9f2b93 17
riannebulthuis 2:866a8a9f2b93 18 void setregelaar_ticker_flag()
riannebulthuis 2:866a8a9f2b93 19 {
riannebulthuis 2:866a8a9f2b93 20 regelaar_ticker_flag = true;
riannebulthuis 2:866a8a9f2b93 21 }
riannebulthuis 2:866a8a9f2b93 22
riannebulthuis 2:866a8a9f2b93 23 #define SAMPLETIME_REGELAAR 0.005
riannebulthuis 2:866a8a9f2b93 24 float referentie_arm1 = 0;
riannebulthuis 2:866a8a9f2b93 25
riannebulthuis 2:866a8a9f2b93 26 //define states
riannebulthuis 2:866a8a9f2b93 27 enum state {GOTOHOMEPOSITION, MOVE_MOTOR, BACKTOHOMEPOSITION};
riannebulthuis 2:866a8a9f2b93 28 uint8_t state = GOTOHOMEPOSITION;
arunr 0:65ab9f79a4cc 29
arunr 0:65ab9f79a4cc 30 const int pressed = 0;
arunr 0:65ab9f79a4cc 31
arunr 0:65ab9f79a4cc 32 double H;
arunr 0:65ab9f79a4cc 33 double P;
arunr 0:65ab9f79a4cc 34 double D;
arunr 0:65ab9f79a4cc 35
arunr 0:65ab9f79a4cc 36
arunr 0:65ab9f79a4cc 37 void sethome(){
arunr 0:65ab9f79a4cc 38 motor1.setPosition(0);
arunr 0:65ab9f79a4cc 39 H = motor1.getPosition();
arunr 0:65ab9f79a4cc 40 }
arunr 0:65ab9f79a4cc 41
arunr 0:65ab9f79a4cc 42 void move_motor1_ccw (){
arunr 0:65ab9f79a4cc 43 motor1_direction = 0;
arunr 0:65ab9f79a4cc 44 motor1_speed = 0.8;
arunr 0:65ab9f79a4cc 45 }
arunr 0:65ab9f79a4cc 46
arunr 0:65ab9f79a4cc 47 void move_motor1_cw (){
arunr 0:65ab9f79a4cc 48 motor1_direction = 1;
arunr 0:65ab9f79a4cc 49 motor1_speed = 0.8;
arunr 0:65ab9f79a4cc 50 }
arunr 0:65ab9f79a4cc 51
arunr 0:65ab9f79a4cc 52 void movetohome(){
arunr 0:65ab9f79a4cc 53 P = motor1.getPosition();
arunr 0:65ab9f79a4cc 54 D = (P - H);
arunr 0:65ab9f79a4cc 55
riannebulthuis 2:866a8a9f2b93 56 if (D > -5 && D < 5){
arunr 0:65ab9f79a4cc 57 motor1_speed = 0;
arunr 0:65ab9f79a4cc 58 }
arunr 0:65ab9f79a4cc 59 else if (D > 0){
arunr 0:65ab9f79a4cc 60 move_motor1_cw();
arunr 0:65ab9f79a4cc 61 }
arunr 0:65ab9f79a4cc 62 else if (D < 0){
arunr 0:65ab9f79a4cc 63 move_motor1_ccw();
arunr 0:65ab9f79a4cc 64 }
arunr 0:65ab9f79a4cc 65 }
arunr 0:65ab9f79a4cc 66
arunr 0:65ab9f79a4cc 67 void move_motor1()
arunr 0:65ab9f79a4cc 68 {
arunr 0:65ab9f79a4cc 69 if (button_1 == pressed) {
arunr 0:65ab9f79a4cc 70 move_motor1_cw ();
riannebulthuis 1:7d5e6bc2b314 71 } else if (button_2 == pressed) {
riannebulthuis 1:7d5e6bc2b314 72 move_motor1_ccw ();
riannebulthuis 1:7d5e6bc2b314 73 } else {
riannebulthuis 1:7d5e6bc2b314 74 motor1_speed = 0;
arunr 0:65ab9f79a4cc 75 }
arunr 0:65ab9f79a4cc 76 }
arunr 0:65ab9f79a4cc 77
arunr 0:65ab9f79a4cc 78 void read_encoder1 () // aflezen van encoder via hidscope??
arunr 0:65ab9f79a4cc 79 {
arunr 0:65ab9f79a4cc 80 scope.set(0,motor1.getPosition());
arunr 0:65ab9f79a4cc 81 led.write(motor1.getPosition()/100.0);
arunr 0:65ab9f79a4cc 82 scope.send();
arunr 0:65ab9f79a4cc 83 wait(0.2f);
arunr 0:65ab9f79a4cc 84 }
arunr 0:65ab9f79a4cc 85
arunr 0:65ab9f79a4cc 86 int main()
riannebulthuis 2:866a8a9f2b93 87 {
arunr 0:65ab9f79a4cc 88 while (true) {
riannebulthuis 1:7d5e6bc2b314 89 pc.baud(9600); //pc baud rate van de computer
riannebulthuis 1:7d5e6bc2b314 90
riannebulthuis 1:7d5e6bc2b314 91 switch (state) { //zorgt voor het in goede volgorde uitvoeren van de cases
riannebulthuis 1:7d5e6bc2b314 92
riannebulthuis 2:866a8a9f2b93 93 case GOTOHOMEPOSITION: //positie op 0 zetten voor arm 1
riannebulthuis 1:7d5e6bc2b314 94 {
riannebulthuis 2:866a8a9f2b93 95 pc.printf("gotohomeposition\n\r");
riannebulthuis 1:7d5e6bc2b314 96 read_encoder1();
riannebulthuis 1:7d5e6bc2b314 97 sethome();
riannebulthuis 2:866a8a9f2b93 98 state = MOVE_MOTOR;
riannebulthuis 1:7d5e6bc2b314 99 break;
riannebulthuis 1:7d5e6bc2b314 100 }
riannebulthuis 1:7d5e6bc2b314 101
riannebulthuis 2:866a8a9f2b93 102 case MOVE_MOTOR: //motor kan cw en ccw bewegen a.d.h.v. buttons
riannebulthuis 1:7d5e6bc2b314 103 {
riannebulthuis 2:866a8a9f2b93 104 pc.printf("move_motor\n\r");
riannebulthuis 2:866a8a9f2b93 105 while (state == MOVE_MOTOR){
riannebulthuis 2:866a8a9f2b93 106 pc.printf("whiletrue\n\r");
riannebulthuis 2:866a8a9f2b93 107 move_motor1();
riannebulthuis 2:866a8a9f2b93 108 if (button_1 == pressed && button_2 == pressed){
riannebulthuis 2:866a8a9f2b93 109 state = BACKTOHOMEPOSITION;
riannebulthuis 2:866a8a9f2b93 110 }
riannebulthuis 2:866a8a9f2b93 111 }
riannebulthuis 2:866a8a9f2b93 112 wait (1);
riannebulthuis 2:866a8a9f2b93 113 break;
riannebulthuis 1:7d5e6bc2b314 114 }
riannebulthuis 1:7d5e6bc2b314 115
riannebulthuis 2:866a8a9f2b93 116 case BACKTOHOMEPOSITION: //motor gaat terug naar homeposition
riannebulthuis 1:7d5e6bc2b314 117 {
riannebulthuis 2:866a8a9f2b93 118 pc.printf("backhomeposition\n\r");
riannebulthuis 2:866a8a9f2b93 119 wait (1);
riannebulthuis 2:866a8a9f2b93 120
riannebulthuis 2:866a8a9f2b93 121 ticker_regelaar.attach(setregelaar_ticker_flag, SAMPLETIME_REGELAAR);
riannebulthuis 2:866a8a9f2b93 122
riannebulthuis 2:866a8a9f2b93 123 while(state == BACKTOHOMEPOSITION){
riannebulthuis 2:866a8a9f2b93 124 while(regelaar_ticker_flag != true);
riannebulthuis 2:866a8a9f2b93 125 regelaar_ticker_flag = false;
riannebulthuis 2:866a8a9f2b93 126
riannebulthuis 2:866a8a9f2b93 127 pc.printf("pulsmotorposition1 %d", motor1.getPosition());
riannebulthuis 2:866a8a9f2b93 128 pc.printf("referentie %f\n\r", referentie_arm1);
riannebulthuis 2:866a8a9f2b93 129
riannebulthuis 2:866a8a9f2b93 130 if (motor1.getPosition() <=24 && motor1.getPosition() >= -24){
riannebulthuis 2:866a8a9f2b93 131 referentie_arm1 = 0;
riannebulthuis 2:866a8a9f2b93 132 }
riannebulthuis 2:866a8a9f2b93 133
riannebulthuis 1:7d5e6bc2b314 134 break;
riannebulthuis 1:7d5e6bc2b314 135 }
riannebulthuis 2:866a8a9f2b93 136 }
riannebulthuis 2:866a8a9f2b93 137 }
arunr 0:65ab9f79a4cc 138 }