Renate de Boer / Mbed 2 deprecated script_voor_project_copy

Script 15-10-2019

Dependencies:   Servoaansturing mbed QEI HIDScope biquadFilter MODSERIAL FastPWM

main.cpp@7:1d57463393c6, 2019-10-11 (annotated)

Committer:
Renate
Date:
Fri Oct 11 12:31:34 2019 +0000
Revision:
7:1d57463393c6
Parent:
6:64146e16e10c
Child:
8:c7d3b67346db
Basis voor finite state machine

Who changed what in which revision?

UserRevisionLine numberNew contents of line
RobertoO 0:67c50348f842 1 #include "mbed.h"
Rosalie 3:6ee0b20c23b0 2 #include "HIDScope.h"
Rosalie 3:6ee0b20c23b0 3 #include "QEI.h"
RobertoO 1:b862262a9d14 4 #include "MODSERIAL.h"
Rosalie 3:6ee0b20c23b0 5 #include "BiQuad.h"
Rosalie 3:6ee0b20c23b0 6 #include "FastPWM.h"
WiesjeRoskamp 2:aee655d11b6d 7 #include <math.h>
Rosalie 5:9f1260408ef2 8 #include "Servo.h"
RobertoO 0:67c50348f842 9
Rosalie 3:6ee0b20c23b0 10 //definieer objecten
WiesjeRoskamp 2:aee655d11b6d 11 Serial pc(USBTX, USBRX);
Rosalie 3:6ee0b20c23b0 12
Rosalie 5:9f1260408ef2 13 //Servo myservo(D8);
Rosalie 5:9f1260408ef2 14
Rosalie 3:6ee0b20c23b0 15 Ticker myControllerTicker1;
Rosalie 3:6ee0b20c23b0 16 Ticker myControllerTicker2;
Rosalie 3:6ee0b20c23b0 17
Rosalie 4:6742476fadcf 18 AnalogIn potMeter1(A1);
Rosalie 4:6742476fadcf 19 AnalogIn potMeter2(A0);
Rosalie 3:6ee0b20c23b0 20
Rosalie 4:6742476fadcf 21 PwmOut motor1(D6);
Rosalie 4:6742476fadcf 22 PwmOut motor2(D5);
Rosalie 3:6ee0b20c23b0 23
Rosalie 4:6742476fadcf 24 DigitalOut motor1_dir(D7);
Rosalie 4:6742476fadcf 25 DigitalOut motor2_dir(D4);
Rosalie 3:6ee0b20c23b0 26
Rosalie 5:9f1260408ef2 27
Rosalie 4:6742476fadcf 28 InterruptIn button1(D1);
Rosalie 4:6742476fadcf 29 InterruptIn button2(D2);
WiesjeRoskamp 2:aee655d11b6d 30
Rosalie 3:6ee0b20c23b0 31 //richting wisselen van motor 1
Rosalie 3:6ee0b20c23b0 32 void direction1(void)
Rosalie 3:6ee0b20c23b0 33 {
Rosalie 3:6ee0b20c23b0 34 motor1_dir=!motor1_dir;
Rosalie 3:6ee0b20c23b0 35 }
Rosalie 3:6ee0b20c23b0 36
Rosalie 3:6ee0b20c23b0 37 //richting wisselen van motor 2
Rosalie 3:6ee0b20c23b0 38 void direction2(void)
Rosalie 3:6ee0b20c23b0 39 {
Rosalie 3:6ee0b20c23b0 40 motor2_dir=!motor2_dir;
Rosalie 3:6ee0b20c23b0 41 }
Rosalie 3:6ee0b20c23b0 42
Rosalie 3:6ee0b20c23b0 43
Rosalie 3:6ee0b20c23b0 44 //snelheid motor 1 aansturen
Rosalie 3:6ee0b20c23b0 45 void motor1Controller( )
WiesjeRoskamp 2:aee655d11b6d 46 {
Rosalie 3:6ee0b20c23b0 47 // Determine reference (desired) fan speed
Rosalie 3:6ee0b20c23b0 48 double y1_des = potMeter1.read();
Rosalie 3:6ee0b20c23b0 49 // Controller (calculate motor torque/pwm)
Rosalie 3:6ee0b20c23b0 50 if( y1_des > 1 ) y1_des = 1; // y1_des must be <= 1
Rosalie 3:6ee0b20c23b0 51 if( y1_des < 0 ) y1_des = 0; // y1_des must be >= 0
Rosalie 3:6ee0b20c23b0 52 double power1 = pow(y1_des, 2.0); // Inverse relation between input and output
Rosalie 3:6ee0b20c23b0 53 // Send to motor
Rosalie 3:6ee0b20c23b0 54 motor1.write( power1 );
Rosalie 3:6ee0b20c23b0 55 pc.printf("power1: %.2f\t",power1);
Rosalie 3:6ee0b20c23b0 56 }
Rosalie 3:6ee0b20c23b0 57
Rosalie 3:6ee0b20c23b0 58 //snelheid motor 2 aansturen
Rosalie 3:6ee0b20c23b0 59 void motor2Controller( )
Rosalie 3:6ee0b20c23b0 60 {
Rosalie 3:6ee0b20c23b0 61 // Determine reference (desired) fan speed
Rosalie 3:6ee0b20c23b0 62 double y2_des = potMeter2.read();
Rosalie 3:6ee0b20c23b0 63 // Controller (calculate motor torque/pwm)
Rosalie 3:6ee0b20c23b0 64 if( y2_des > 1 ) y2_des = 1; // y2_des must be <= 1
Rosalie 3:6ee0b20c23b0 65 if( y2_des < 0 ) y2_des = 0; // y2_des must be >= 0
Rosalie 3:6ee0b20c23b0 66 double power2 = pow(y2_des, 2.0); // Inverse relation between input and output
Rosalie 3:6ee0b20c23b0 67 // Send to motor
Rosalie 3:6ee0b20c23b0 68 motor2.write( power2 );
Rosalie 3:6ee0b20c23b0 69 pc.printf("power2: %.2f\n\r",power2);
Rosalie 3:6ee0b20c23b0 70 }
Rosalie 3:6ee0b20c23b0 71
Renate 6:64146e16e10c 72 // Finite state machine programming (calibration servo motor?)
Renate 7:1d57463393c6 73 enum states {Motors_off, Calib_motor, Calib_EMG, Homing, Operation_mode, Failure_mode};
Renate 6:64146e16e10c 74
Renate 6:64146e16e10c 75 states currentState = Motors_off;
Renate 6:64146e16e10c 76 bool stateChanged = true; // Make sure the initialization of first state is executed
Renate 6:64146e16e10c 77
Renate 6:64146e16e10c 78
Renate 6:64146e16e10c 79 void ProcessStateMachine(void)
Renate 6:64146e16e10c 80 {
Renate 6:64146e16e10c 81 switch (currentState)
Renate 6:64146e16e10c 82 {
Renate 6:64146e16e10c 83 case Motors_off:
Renate 6:64146e16e10c 84
Renate 6:64146e16e10c 85 if (stateChanged)
Renate 6:64146e16e10c 86 {
Renate 6:64146e16e10c 87 // functie toevoegen waarbij motoren uitgaan
Renate 6:64146e16e10c 88 stateChanged = false;
Renate 6:64146e16e10c 89 pc.printf("Motors off\n");
Renate 6:64146e16e10c 90 }
Renate 7:1d57463393c6 91 if // (Power_button_pressed()) nog een button toevoegen voordat dit kan
Renate 6:64146e16e10c 92 {
Renate 6:64146e16e10c 93 currentState = Calib_motor;
Renate 6:64146e16e10c 94 stateChanged = true;
Renate 6:64146e16e10c 95 pc.printf("Moving to Calib_motor state\n");
Renate 6:64146e16e10c 96 }
Renate 7:1d57463393c6 97 if // (Emergency_button_pressed()) nog een button toevoegen voordat dit kan
Renate 7:1d57463393c6 98 {
Renate 7:1d57463393c6 99 currentState = Failure_mode;
Renate 7:1d57463393c6 100 stateChanged = true;
Renate 7:1d57463393c6 101 pc.printf("Moving to failure_mode\n");
Renate 7:1d57463393c6 102 }
Renate 7:1d57463393c6 103
Renate 6:64146e16e10c 104 break;
Renate 6:64146e16e10c 105
Renate 6:64146e16e10c 106 case Calib_motor:
Renate 6:64146e16e10c 107
Renate 6:64146e16e10c 108 if (stateChanged)
Renate 6:64146e16e10c 109 {
Renate 6:64146e16e10c 110 // Hier wordt een kalibratie uitgevoerd, waarbij de motorhoeken worden bepaald
Renate 6:64146e16e10c 111 currentState = Calib_EMG;
Renate 6:64146e16e10c 112 stateChanged = true;
Renate 6:64146e16e10c 113 pc.printf("Moving to Calib_EMG state\n");
Renate 7:1d57463393c6 114 }
Renate 7:1d57463393c6 115 if // (Emergency_button_pressed()) nog een button toevoegen voordat dit kan
Renate 7:1d57463393c6 116 {
Renate 7:1d57463393c6 117 currentState = Failure_mode;
Renate 7:1d57463393c6 118 stateChanged = true;
Renate 7:1d57463393c6 119 pc.printf("Moving to failure_mode\n");
Renate 7:1d57463393c6 120 }
Renate 7:1d57463393c6 121
Renate 6:64146e16e10c 122 break;
Renate 6:64146e16e10c 123
Renate 6:64146e16e10c 124 case Calib_EMG:
Renate 6:64146e16e10c 125
Renate 6:64146e16e10c 126 if (stateChanged)
Renate 6:64146e16e10c 127 {
Renate 6:64146e16e10c 128 // Hierbij wordt een een kalibratie uitgevoerd, waarbij de maximale EMG-amplitude waarde wordt bepaald
Renate 6:64146e16e10c 129 currentState = Homing;
Renate 6:64146e16e10c 130 stateChanged = true;
Renate 6:64146e16e10c 131 pc.printf("Moving to Homing state\n");
Renate 6:64146e16e10c 132 }
Renate 7:1d57463393c6 133 if // (Emergency_button_pressed()) nog een button toevoegen voordat dit kan
Renate 7:1d57463393c6 134 {
Renate 7:1d57463393c6 135 currentState = Failure_mode;
Renate 7:1d57463393c6 136 stateChanged = true;
Renate 7:1d57463393c6 137 pc.printf("Moving to failure_mode\n");
Renate 7:1d57463393c6 138 }
Renate 7:1d57463393c6 139
Renate 6:64146e16e10c 140 break;
Renate 6:64146e16e10c 141
Renate 6:64146e16e10c 142 case Homing:
Renate 6:64146e16e10c 143
Renate 6:64146e16e10c 144 if (stateChanged)
Renate 6:64146e16e10c 145 {
Renate 7:1d57463393c6 146 // Ervoor zorgen dat de motoren zo bewegen dat de robotarm
Renate 7:1d57463393c6 147 // (inclusief de end-effector) in de juiste home positie wordt gezet
Renate 7:1d57463393c6 148 currentState = Operation_mode;
Renate 7:1d57463393c6 149 stateChanged = true;
Renate 7:1d57463393c6 150 pc.printf("Moving to operation mode\n");
Renate 7:1d57463393c6 151 }
Renate 7:1d57463393c6 152 if // (Emergency_button_pressed()) nog een button toevoegen voordat dit kan
Renate 7:1d57463393c6 153 {
Renate 7:1d57463393c6 154 currentState = Failure_mode;
Renate 6:64146e16e10c 155 stateChanged = true;
Renate 7:1d57463393c6 156 pc.printf("Moving to failure_mode\n");
Renate 7:1d57463393c6 157 }
Renate 7:1d57463393c6 158 break;
Renate 7:1d57463393c6 159
Renate 7:1d57463393c6 160 case Operation_mode:
Renate 7:1d57463393c6 161
Renate 7:1d57463393c6 162 if (stateChanged)
Renate 7:1d57463393c6 163 {
Renate 7:1d57463393c6 164 // Hier moet een functie worden aangeroepen die ervoor zorgt dat
Renate 7:1d57463393c6 165 // aan de hand van EMG-signalen de motoren kunnen worden aangestuurd,
Renate 7:1d57463393c6 166 // zodat de robotarm kan bewegen
Renate 7:1d57463393c6 167
Renate 7:1d57463393c6 168 if // (Power_button_pressed())
Renate 7:1d57463393c6 169 {
Renate 7:1d57463393c6 170 currentState = Motors_off;
Renate 7:1d57463393c6 171 stateChanged = true;
Renate 7:1d57463393c6 172 }
Renate 7:1d57463393c6 173 if // (Emergency_button_pressed()) nog een button toevoegen voordat dit kan
Renate 7:1d57463393c6 174 {
Renate 7:1d57463393c6 175 currentState = Failure_mode;
Renate 7:1d57463393c6 176 stateChanged = true;
Renate 7:1d57463393c6 177 pc.printf("Moving to failure_mode\n");
Renate 7:1d57463393c6 178 }
Renate 7:1d57463393c6 179 else
Renate 7:1d57463393c6 180 {
Renate 7:1d57463393c6 181 currentState = Homing;
Renate 7:1d57463393c6 182 stateChanged = true;
Renate 7:1d57463393c6 183 }
Renate 7:1d57463393c6 184 break;
Renate 7:1d57463393c6 185
Renate 7:1d57463393c6 186 case Failure_mode:
Renate 7:1d57463393c6 187
Renate 7:1d57463393c6 188 if (stateChanged)
Renate 7:1d57463393c6 189 {
Renate 7:1d57463393c6 190 pc.printf("Ik ga exploderen!!!");
Renate 7:1d57463393c6 191 // Alles moet uitgaan (evt. een rood LEDje laten branden), moet
Renate 7:1d57463393c6 192 // opnieuw worden opgestart. Mogelijk kan dit door de ticker de
Renate 7:1d57463393c6 193 // detachen
Renate 6:64146e16e10c 194 }
Renate 6:64146e16e10c 195 break;
Renate 6:64146e16e10c 196
Renate 7:1d57463393c6 197 default:
Renate 7:1d57463393c6 198 // Zelfde functie als die eerder is toegepast om motoren uit te schakelen -> safety!
Renate 7:1d57463393c6 199 pc.printf("Unknown or uninplemented state reached!\n");
Renate 7:1d57463393c6 200 }
Rosalie 3:6ee0b20c23b0 201
Rosalie 3:6ee0b20c23b0 202 int main()
Rosalie 3:6ee0b20c23b0 203 {
Rosalie 5:9f1260408ef2 204 // for(float p=0; p<1.0; p += 0.1)
Rosalie 5:9f1260408ef2 205 // {
Rosalie 5:9f1260408ef2 206 // myservo == p;
Rosalie 5:9f1260408ef2 207 // wait(0.2);
Rosalie 5:9f1260408ef2 208 // }
Rosalie 3:6ee0b20c23b0 209 myControllerTicker1.attach(motor1Controller, 0.1 ); // Every 1/10 second
Rosalie 3:6ee0b20c23b0 210 myControllerTicker2.attach(motor2Controller, 0.1 ); // Every 1/10 second
Rosalie 3:6ee0b20c23b0 211
Rosalie 3:6ee0b20c23b0 212 button1.mode(PullUp);
Rosalie 3:6ee0b20c23b0 213 button1.rise(direction1);
Rosalie 3:6ee0b20c23b0 214 button2.mode(PullUp);
Rosalie 3:6ee0b20c23b0 215 button2.rise(direction2);
Rosalie 3:6ee0b20c23b0 216
Rosalie 3:6ee0b20c23b0 217
Rosalie 3:6ee0b20c23b0 218 while( true ) { /* do nothing */ }
Rosalie 3:6ee0b20c23b0 219 return 0;
WiesjeRoskamp 2:aee655d11b6d 220 }
WiesjeRoskamp 2:aee655d11b6d 221
WiesjeRoskamp 2:aee655d11b6d 222
WiesjeRoskamp 2:aee655d11b6d 223
RobertoO 0:67c50348f842 224