Renate de Boer / Mbed 2 deprecated script_voor_project_copy

Script 15-10-2019

Dependencies:   Servoaansturing mbed QEI HIDScope biquadFilter MODSERIAL FastPWM

main.cpp@6:64146e16e10c, 2019-10-11 (annotated)

Committer:
Renate
Date:
Fri Oct 11 10:48:24 2019 +0000
Revision:
6:64146e16e10c
Parent:
5:9f1260408ef2
Child:
7:1d57463393c6
Versie met knoppen (half)

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 6:64146e16e10c 73 enum states {Motors_off, Calib_motor, Calib_EMG, Homing, No_mov_before_pick_up, Servo_horizontal, Position_1, Slide, Lift, No_mov_before_drop_off, Position_drop_off, Servo_tilt_down, Position_2};
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 6:64146e16e10c 91 if // (Motor_calib_button_pressed()) nog een extra 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 6:64146e16e10c 97 break;
Renate 6:64146e16e10c 98
Renate 6:64146e16e10c 99 case Calib_motor:
Renate 6:64146e16e10c 100
Renate 6:64146e16e10c 101 if (stateChanged)
Renate 6:64146e16e10c 102 {
Renate 6:64146e16e10c 103 // Hier wordt een kalibratie uitgevoerd, waarbij de motorhoeken worden bepaald
Renate 6:64146e16e10c 104 currentState = Calib_EMG;
Renate 6:64146e16e10c 105 stateChanged = true;
Renate 6:64146e16e10c 106 pc.printf("Moving to Calib_EMG state\n");
Renate 6:64146e16e10c 107 }
Renate 6:64146e16e10c 108 break;
Renate 6:64146e16e10c 109
Renate 6:64146e16e10c 110 case Calib_EMG:
Renate 6:64146e16e10c 111
Renate 6:64146e16e10c 112 if (stateChanged)
Renate 6:64146e16e10c 113 {
Renate 6:64146e16e10c 114 // Hierbij wordt een een kalibratie uitgevoerd, waarbij de maximale EMG-amplitude waarde wordt bepaald
Renate 6:64146e16e10c 115 currentState = Homing;
Renate 6:64146e16e10c 116 stateChanged = true;
Renate 6:64146e16e10c 117 pc.printf("Moving to Homing state\n");
Renate 6:64146e16e10c 118 }
Renate 6:64146e16e10c 119 break;
Renate 6:64146e16e10c 120
Renate 6:64146e16e10c 121 case Homing:
Renate 6:64146e16e10c 122
Renate 6:64146e16e10c 123 if (stateChanged)
Renate 6:64146e16e10c 124 {
Renate 6:64146e16e10c 125 // Ervoor zorgen dat de motoren zo bewegen dat de robotarm in de juiste home positie wordt gezet
Renate 6:64146e16e10c 126 currentState = No_mov_before_pick_up;
Renate 6:64146e16e10c 127 stateChanged = true;
Renate 6:64146e16e10c 128 pc.printf("Moving to no movement before pick up state \n");
Renate 6:64146e16e10c 129 }
Renate 6:64146e16e10c 130 break;
Renate 6:64146e16e10c 131
Renate 6:64146e16e10c 132 case No_mov_before_pick_up: // De robot arm beweegt niet, soort ruststand na de homing
Renate 6:64146e16e10c 133
Renate 6:64146e16e10c 134 if (stateChanged)
Renate 6:64146e16e10c 135 {
Renate 6:64146e16e10c 136 if (// eis om naar positie 1 te gaan)
Renate 6:64146e16e10c 137 {
Renate 6:64146e16e10c 138 currentState = Position_1
Renate 6:64146e16e10c 139 stateChanged = true;
Renate 6:64146e16e10c 140 pc.printf("Moving to position_1 state\n");
Renate 6:64146e16e10c 141 }
Renate 6:64146e16e10c 142
Renate 6:64146e16e10c 143 if (// eis om naar positie 2 te gaan)
Renate 6:64146e16e10c 144 {
Renate 6:64146e16e10c 145 currentState = Position_2;
Renate 6:64146e16e10c 146 stateChanged = true;
Renate 6:64146e16e10c 147 pc.printf("Moving to position_2 state\n");
Renate 6:64146e16e10c 148 }
Renate 6:64146e16e10c 149
Renate 6:64146e16e10c 150 if (// eis om naar drop off positie te gaan, als het eraf glijden mislukt is)
Renate 6:64146e16e10c 151 {
Renate 6:64146e16e10c 152 currentState = Position_drop_off;
Renate 6:64146e16e10c 153 stateChanged = true;
Renate 6:64146e16e10c 154 pc.printf("Moving to position_drop_off state\n");
Renate 6:64146e16e10c 155 }
Renate 6:64146e16e10c 156
Renate 6:64146e16e10c 157 if (// eis voor nieuwe poging om home positie juist te bereiken)
Renate 6:64146e16e10c 158 {
Renate 6:64146e16e10c 159 currentState = Homing; // Mogelijk de state Calib_motor
Renate 6:64146e16e10c 160 stateChanged = true;
Renate 6:64146e16e10c 161 pc.printf("Retry homing\n");
Renate 6:64146e16e10c 162 }
Renate 6:64146e16e10c 163
Renate 6:64146e16e10c 164 if (// eis om de motoren uit te zetten)
Renate 6:64146e16e10c 165 {
Renate 6:64146e16e10c 166 currentState = Motors_off
Renate 6:64146e16e10c 167 stateChanged = true;
Renate 6:64146e16e10c 168 pc.printf("Shutting down\n");
Renate 6:64146e16e10c 169 }
Renate 6:64146e16e10c 170 }
Renate 6:64146e16e10c 171 break;
Renate 6:64146e16e10c 172
Renate 6:64146e16e10c 173 case Position_1:
Renate 6:64146e16e10c 174
Renate 6:64146e16e10c 175 if (stateChanged)
Renate 6:64146e16e10c 176 {
Renate 6:64146e16e10c 177 // Ervoor zorgen dat de end-effector
Renate 6:64146e16e10c 178
Renate 6:64146e16e10c 179
Renate 6:64146e16e10c 180
Renate 6:64146e16e10c 181 if (stateChanged)
Renate 6:64146e16e10c 182 {
Renate 6:64146e16e10c 183 // Ervoor zorgen dat de motoren zo bewegen dat de robotarm in de juiste home positie wordt gezet
Renate 6:64146e16e10c 184 currentState = No_mov_before_pick_up;
Renate 6:64146e16e10c 185 stateChanged = true;
Renate 6:64146e16e10c 186 pc.printf("Moving to no movement before pick up state \n");
Renate 6:64146e16e10c 187 }
Renate 6:64146e16e10c 188 break;
Renate 6:64146e16e10c 189
Renate 6:64146e16e10c 190
Rosalie 3:6ee0b20c23b0 191
Rosalie 3:6ee0b20c23b0 192 int main()
Rosalie 3:6ee0b20c23b0 193 {
Rosalie 5:9f1260408ef2 194 // for(float p=0; p<1.0; p += 0.1)
Rosalie 5:9f1260408ef2 195 // {
Rosalie 5:9f1260408ef2 196 // myservo == p;
Rosalie 5:9f1260408ef2 197 // wait(0.2);
Rosalie 5:9f1260408ef2 198 // }
Rosalie 3:6ee0b20c23b0 199 myControllerTicker1.attach(motor1Controller, 0.1 ); // Every 1/10 second
Rosalie 3:6ee0b20c23b0 200 myControllerTicker2.attach(motor2Controller, 0.1 ); // Every 1/10 second
Rosalie 3:6ee0b20c23b0 201
Rosalie 3:6ee0b20c23b0 202 button1.mode(PullUp);
Rosalie 3:6ee0b20c23b0 203 button1.rise(direction1);
Rosalie 3:6ee0b20c23b0 204 button2.mode(PullUp);
Rosalie 3:6ee0b20c23b0 205 button2.rise(direction2);
Rosalie 3:6ee0b20c23b0 206
Rosalie 3:6ee0b20c23b0 207
Rosalie 3:6ee0b20c23b0 208 while( true ) { /* do nothing */ }
Rosalie 3:6ee0b20c23b0 209 return 0;
WiesjeRoskamp 2:aee655d11b6d 210 }
WiesjeRoskamp 2:aee655d11b6d 211
WiesjeRoskamp 2:aee655d11b6d 212
WiesjeRoskamp 2:aee655d11b6d 213
RobertoO 0:67c50348f842 214