Projectgroep 20 Biorobotics / Mbed 2 deprecated cases2

opzetje van cases

Dependencies:   mbed

main.cpp@1:be295a50eb53, 2017-11-01 (annotated)

Committer:
charloverwijk
Date:
Wed Nov 01 13:58:51 2017 +0000
Revision:
1:be295a50eb53
Parent:
0:0a4a244cb867 
State Machine

Who changed what in which revision?

UserRevisionLine numberNew contents of line
charloverwijk 0:0a4a244cb867 1 #include "mbed.h"
charloverwijk 0:0a4a244cb867 2
charloverwijk 1:be295a50eb53 3 enum States (Cal1, Cal2, CalEMG, Home, EMG, Rest, Demo);
charloverwijk 0:0a4a244cb867 4 int State, Counter;
charloverwijk 0:0a4a244cb867 5 bool Position_controller_on;
charloverwijk 0:0a4a244cb867 6 float Looptime = 0.002f;
charloverwijk 1:be295a50eb53 7 volatile vorigepositie
charloverwijk 0:0a4a244cb867 8
charloverwijk 0:0a4a244cb867 9 int main()
charloverwijk 0:0a4a244cb867 10 {
charloverwijk 0:0a4a244cb867 11 State = Cal1;
charloverwijk 0:0a4a244cb867 12 Position_controller_on = false;
charloverwijk 0:0a4a244cb867 13 Counter = 0;
charloverwijk 0:0a4a244cb867 14
charloverwijk 0:0a4a244cb867 15 Main_loop.attach(&loop_function, looptime);
charloverwijk 0:0a4a244cb867 16 }
charloverwijk 0:0a4a244cb867 17
charloverwijk 0:0a4a244cb867 18 void Loop_funtion()
charloverwijk 0:0a4a244cb867 19 {
charloverwijk 0:0a4a244cb867 20 switch(State){
charloverwijk 1:be295a50eb53 21 case Cal1: //Calibration motor 1
charloverwijk 1:be295a50eb53 22 if (Huidigepositie1== 0)
charloverwijk 1:be295a50eb53 23 {
charloverwijk 1:be295a50eb53 24 SetMotor1(value); //value is waarde encoder voor loodrechte hoeken,.
charloverwijk 1:be295a50eb53 25 if (fabs(huidigepositie1-home1)<0.01) {
charloverwijk 1:be295a50eb53 26 state=Cal2
charloverwijk 1:be295a50eb53 27 }
charloverwijk 1:be295a50eb53 28 }
charloverwijk 1:be295a50eb53 29 else {
charloverwijk 1:be295a50eb53 30 SetMotor1(0);
charloverwijk 1:be295a50eb53 31 Loop_function();
charloverwijk 1:be295a50eb53 32 }
charloverwijk 0:0a4a244cb867 33 break;
charloverwijk 0:0a4a244cb867 34
charloverwijk 1:be295a50eb53 35 case Cal2: //Calibration motor 2
charloverwijk 1:be295a50eb53 36 if (Huidigepositie2== 0)
charloverwijk 1:be295a50eb53 37 {
charloverwijk 1:be295a50eb53 38 if (encoder2.read)<0.01){
charloverwijk 1:be295a50eb53 39 state=CalEMG;
charloverwijk 1:be295a50eb53 40 }
charloverwijk 1:be295a50eb53 41 else {
charloverwijk 1:be295a50eb53 42 SetMotor2(0);
charloverwijk 1:be295a50eb53 43 Loop_function();
charloverwijk 1:be295a50eb53 44 }
charloverwijk 1:be295a50eb53 45 break;
charloverwijk 1:be295a50eb53 46 case CalEMG: // Calibration EMG
charloverwijk 1:be295a50eb53 47 calibrationEMG(); //calculates average EMGFiltered at rest and measures max signal EMGFiltered.
charloverwijk 1:be295a50eb53 48 //measure mean at restand substract always--> no treshold needed.
charloverwijk 1:be295a50eb53 49 //1/MVC
charloverwijk 1:be295a50eb53 50 state=SelectDevice;
charloverwijk 1:be295a50eb53 51 break;
charloverwijk 1:be295a50eb53 52 case SelectDevice: //Looks at the difference between current position and home. Select aansturen EMG or buttons
charloverwijk 1:be295a50eb53 53 if button=1; {
charloverwijk 1:be295a50eb53 54 state=EMG;
charloverwijk 1:be295a50eb53 55 }
charloverwijk 1:be295a50eb53 56 if button=0; {
charloverwijk 1:be295a50eb53 57 state=Demo;
charloverwijk 1:be295a50eb53 58 }
charloverwijk 0:0a4a244cb867 59 break;
charloverwijk 0:0a4a244cb867 60
charloverwijk 1:be295a50eb53 61 case EMG: //Aansturen met EMG
charloverwijk 0:0a4a244cb867 62 break;
charloverwijk 1:be295a50eb53 63 case Demo: // Aansturen met toetsenbord
charloverwijk 0:0a4a244cb867 64 break;
charloverwijk 0:0a4a244cb867 65 }
charloverwijk 0:0a4a244cb867 66
charloverwijk 0:0a4a244cb867 67 //after this the kinematics and control wil come
charloverwijk 0:0a4a244cb867 68 }