Jesse Kaiser
/
Motorscript_Gr6
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: Encoder HIDScope MODSERIAL mbed-dsp mbed
Fork of
Lampje_EMG_Gr6
by 
Iris van Leeuwen
Revision 20:d40b6cba4280, committed 2014-10-29
- Comitter:
- irisl
- Date:
- Wed Oct 29 15:08:30 2014 +0000
- Parent:
- 19:1bd2fc3bce1e
- Child:
- 21:674fafb6301d
- Commit message:
- Met werkende motor in de presets
Changed in this revision
| Encoder.lib | Show annotated file Show diff for this revision Revisions of this file |
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Encoder.lib Wed Oct 29 15:08:30 2014 +0000 @@ -0,0 +1,1 @@ +https://mbed.org/users/vsluiter/code/Encoder/#18b000b443af
--- a/main.cpp Wed Oct 29 08:59:07 2014 +0000
+++ b/main.cpp Wed Oct 29 15:08:30 2014 +0000
@@ -2,18 +2,57 @@
#include "HIDScope.h"
#include "arm_math.h"
#include "MODSERIAL.h"
+#include "encoder.h"
-Serial pc(USBTX, USBRX); // tx, rx
+#define TSAMP 0.01
+#define K_P (0.1)
+#define K_I (0.03 *TSAMP)
+#define K_D (0.001 /TSAMP)
+#define I_LIMIT 1.
+
+#define M1_PWM PTC8
+#define M1_DIR PTC9
+#define M2_PWM PTA5
+#define M2_DIR PTA4
+
+//Groene kabel moet op de GROUND en blauw op de 3.3v aansluiting
+
+Serial pc(USBTX, USBRX);
+
DigitalOut myled1(LED_RED);
DigitalOut myled2(LED_GREEN);
DigitalOut myled3(LED_BLUE);
-PwmOut motorsignal(PTD4);
//Define objects
AnalogIn emg0(PTB1); //Analog input
AnalogIn emg1(PTB2); //Analog input
HIDScope scope(2);
+//motor 25D
+Encoder motor1(PTD3,PTD5,true); //wit, geel
+PwmOut pwm_motor1(M2_PWM);
+DigitalOut motordir1(M2_DIR);
+
+//motor2 37D
+Encoder motor2(PTD2, PTD0,true); //wit, geel
+PwmOut pwm_motor2(M1_PWM);
+DigitalOut motordir2(M1_DIR);
+
+float speed1;
+float hoek1;
+float speed2;
+float hoek2;
+
+bool flip=false;
+
+void attime()
+{
+ flip = !flip;
+}
+
+
+// EMG
+
arm_biquad_casd_df1_inst_f32 lowpass_biceps;
arm_biquad_casd_df1_inst_f32 lowpass_deltoid;
//lowpass filter settings: Fc = 225 Hz, Fs = 500 Hz, Gain = -3 dB
@@ -211,6 +250,90 @@
myled3 = 1;
}
+// MOTORFUNCTIES
+
+void motor2_speed_low ()
+{
+ wait(1);
+ speed2 = 1;
+ motordir2=1;
+ pwm_motor2.write(0.3); //Deze snelheid kan lager worden ingesteld om accurator te zijn.
+ wait(0.3); //naar 140 graden
+ pwm_motor2.write(0); //CCW
+ wait(1);
+ motordir2=0;
+ pwm_motor2.write(speed2); //Deze snelheid moet maximaal om het balletje te slaan.
+ wait(0.20); //balletje slaan, 160 graden
+ pwm_motor2.write(0);
+ wait(1);
+ motordir2=1; //CW
+ pwm_motor2.write(0.1); //Deze kan lager worden ingesteld om accurater terug te gaan.
+ wait(1); //terug naar begin positie, 20 graden
+ pwm_motor2.write(0);
+}
+
+void motor2_speed_mid ()
+{
+ wait(1);
+ speed2 = 1;
+ motordir2=1;
+ pwm_motor2.write(0.3); //Deze snelheid kan lager worden ingesteld om accurator te zijn.
+ wait(0.3); //naar 140 graden
+ pwm_motor2.write(0); //CCW
+ wait(1);
+ motordir2=0;
+ pwm_motor2.write(speed2); //Deze snelheid moet maximaal om het balletje te slaan.
+ wait(0.20); //balletje slaan, 160 graden
+ pwm_motor2.write(0);
+ wait(1);
+ motordir2=1; //CW
+ pwm_motor2.write(0.1); //Deze kan lager worden ingesteld om accurater terug te gaan.
+ wait(1); //terug naar begin positie, 20 graden
+ pwm_motor2.write(0);
+}
+
+void motor2_speed_high ()
+{
+ wait(1);
+ speed2 = 1;
+ motordir2=1;
+ pwm_motor2.write(0.3); //Deze snelheid kan lager worden ingesteld om accurator te zijn.
+ wait(0.3); //naar 140 graden
+ pwm_motor2.write(0); //CCW
+ wait(1);
+ motordir2=0;
+ pwm_motor2.write(speed2); //Deze snelheid moet maximaal om het balletje te slaan.
+ wait(0.20); //balletje slaan, 160 graden
+ pwm_motor2.write(0);
+ wait(1);
+ motordir2=1; //CW
+ pwm_motor2.write(0.1); //Deze kan lager worden ingesteld om accurater terug te gaan.
+ wait(1); //terug naar begin positie, 20 graden
+ pwm_motor2.write(0);
+}
+
+void motor1_links()
+{
+ speed1 = 0.7;
+ hoek1 = 0.09; //in seconde
+ wait(1);
+ motordir1=0; //aangeven van directie (0 = CCW)
+ pwm_motor1.write(speed1); //snelheid van de motor
+ wait(hoek1); //Hierdoor kun je het aantal graden bepalen die de as draait
+ pwm_motor1.write(0);
+}
+
+
+void motor1_rechts()
+{
+ speed1 = 0.7;
+ hoek1 = 0.09; //in seconde
+ wait(1);
+ motordir1=1; //aangeven van directie (1 = CW)
+ pwm_motor1.write(speed1); //snelheid van de motor
+ wait(hoek1); //Hierdoor kun je het aantal graden bepalen die de as draait
+ pwm_motor1.write(0);
+}
int main()
{
@@ -232,6 +355,7 @@
/*Everything is handled by the interrupt routine now!*/
while(1) {
+ static float count = 0;
pc.printf("Span de biceps aan om het instellen te starten.\n");
do {
ShineRed();
@@ -241,10 +365,10 @@
BlinkGreen();
while (1) {
pc.printf("In de loop.\n");
- if (filtered_average_bi > 0.05 && filtered_average_del < 0.05) {
+ if (filtered_average_bi > 0.05 && filtered_average_del > 0.05) {
stopblinkgreen();
- pc.printf("Shinered.\n");
- ShineRed();
+ pc.printf("ShineGreen.\n");
+ ShineGreen();
wait (4);
break;
}
@@ -252,14 +376,16 @@
stopblinkgreen();
pc.printf("ShineBlue.\n");
ShineBlue();
+ motor1_links();
wait(4);
break;
- } else if (filtered_average_bi > 0.05 && filtered_average_del > 0.05)
+ } else if (filtered_average_bi > 0.05 && filtered_average_del < 0.05)
{
stopblinkgreen();
- pc.printf("ShineGreen.\n");
- ShineGreen();
+ pc.printf("ShineRed.\n");
+ ShineRed();
+ motor1_rechts();
wait (4);
break;
}
@@ -267,10 +393,11 @@
BlinkGreen();
while (1) {
pc.printf("In de loop.\n");
- if (filtered_average_bi > 0.05 && filtered_average_del < 0.05) {
+ if (filtered_average_bi > 0.05 && filtered_average_del > 0.05) {
stopblinkgreen();
- pc.printf("Shinered.\n");
- ShineRed();
+ pc.printf("ShineGreen.\n");
+ ShineGreen();
+ motor2_speed_mid ();
wait (4);
break;
}
@@ -278,14 +405,16 @@
stopblinkgreen();
pc.printf("ShineBlue.\n");
ShineBlue();
+ motor2_speed_low ();
wait(4);
break;
- } else if (filtered_average_bi > 0.05 && filtered_average_del > 0.05)
+ } else if (filtered_average_bi > 0.05 && filtered_average_del < 0.05)
{
stopblinkgreen();
- pc.printf("ShineGreen.\n");
- ShineGreen();
+ pc.printf("ShineRed.\n");
+ ShineRed();
+ motor2_speed_high ();
wait (4);
break;
}