Aukie Hooglugt
richting is nog niet ingesteld, kan aan setpoint liggen
Dependencies: Encoder HIDScope MODSERIAL- mbed-dsp mbed
Diff: PROJECT_main.cpp
- Revision:
- 0:99cbc87af37c
- Child:
- 1:d44a866de64f
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/PROJECT_main.cpp Fri Oct 31 10:31:02 2014 +0000
@@ -0,0 +1,644 @@
+#include "mbed.h"
+#include "MODSERIAL.h"
+#include "HIDScope.h"
+#include "arm_math.h"
+#include "encoder.h"
+
+#define TSAMP 0.001 // sample freq encoder motor
+#define TIMEB4NEXTCHOICE 1 // sec keuzelampje blijft aan
+#define TIMEBETWEENBLINK 200 // sec voor volgende blink
+#define TSAMP_EMG 0.002 //sample frequency emg
+#define KALIBRATIONTIME 1000 // 10 sec voor bepalen van maximale biceps/triceps waarde
+
+//Define objects
+AnalogIn emg0(PTB1); //Analog input biceps
+AnalogIn emg1(PTB2); //Analog input triceps
+
+Ticker log_timer; //sample emg
+Ticker blink; //ledjes aan/uit
+Ticker looptimer; //motor regelaar
+
+MODSERIAL pc(USBTX,USBRX);
+
+arm_biquad_casd_df1_inst_f32 bihighpass;
+float bihighpass_const[] = {0.8751821104711265, -1.750364220942253, 0.8751821104711265, 1.7347238224240125 , -0.7660046194604936}; //highpass, Fc: 15 Hz, Fsample: 500Hz, Q = 0.7071
+float bihighpass_states[4];
+
+arm_biquad_casd_df1_inst_f32 binotch;
+float binotch_const[] = {0.9714498065192796, -1.5718388053127037, 0.9714498065192796, 1.5718388053127037 , -0.9428996130385592}; //notch, Fc: 50 Hz, Fsample: 500Hz, Q = 10
+float binotch_states[4];
+
+arm_biquad_casd_df1_inst_f32 trihighpass;
+float trihighpass_const[] = {0.8751821104711265, -1.750364220942253, 0.8751821104711265, 1.7347238224240125 , -0.7660046194604936}; //highpass, Fc: 15 Hz, Fsample: 500Hz, Q = 0.7071
+float trihighpass_states[4];
+
+arm_biquad_casd_df1_inst_f32 trinotch;
+float trinotch_const[] = {0.9714498065192796, -1.5718388053127037, 0.9714498065192796, 1.5718388053127037 , -0.9428996130385592}; //notch, Fc: 50 Hz, Fsample: 500Hz, Q = 10
+float trinotch_states[4];
+
+float bi_result = 0;
+float tri_result = 0;
+
+float bi_max = 0;
+float tri_max = 0;
+
+// variables for biceps MAF
+float y0 = 0;
+float y1 = 0;
+float y2 = 0;
+float y3 = 0;
+float y4 = 0;
+float y5 = 0;
+float y6 = 0;
+float y7 = 0;
+float y8 = 0;
+float y9 = 0;
+
+// variables for triceps MAF
+float x0 = 0;
+float x1 = 0;
+float x2 = 0;
+float x3 = 0;
+float x4 = 0;
+float x5 = 0;
+float x6 = 0;
+float x7 = 0;
+float x8 = 0;
+float x9 = 0;
+
+//LED interface
+DigitalOut dir1(PTA1);
+DigitalOut dir2(PTA2);
+DigitalOut dir3(PTD4);
+DigitalOut for1(PTA12);
+DigitalOut for2(PTA13);
+DigitalOut for3(PTD1);
+
+uint8_t direction = 0;
+uint8_t force = 0;
+
+//motorcontrol objects
+
+//motor 1, voltage pins op M2
+Encoder motor1(PTD3, PTD5);
+DigitalOut motor1dir(PTC9);
+PwmOut pwm_motor1(PTC8);
+
+//motor 2, voltage pins op M1
+Encoder motor2(PTD2,PTD0);
+DigitalOut motor2dir(PTA4);
+PwmOut pwm_motor2(PTA5);
+
+float integral = 0;
+float batjeset = 0; //een if statement wordt "true" wanneer batje voor een bepaalde tijd de juiste hoek heeft behouden
+float balhit = 0; //balhit wordt 1 wanneer arm een bepaalde hoek heeft afgelegd
+float controlerror = 0;
+float pwm = 0;
+
+float omrekenfactor1 = 0.0028035714; // 6.28/(32*70)
+float omrekenfactor2 = 0.0015213178; // 6.28/(24*172);
+
+float setpoint1 = 8; // te behalen speed van motor1 (37D)
+float setpoint2 = 3.14; // te behalen hoek van motor2 (25D)
+
+//float setpoint1 = 0; eigenlijk moeten deze waarden later in de if-statement bij motorcontrol bepaald worden
+//float setpoint2 = 0;
+
+float Kp1 = 1.10; //DEZE KP1 EN KP1 ZIJN NOG NIET DEFINITIEF
+float Ki1 = 0.20; //Kp en Ki van motor1, voor de slag
+
+float Kp2 = 0.30; //Kp en Ki van motor2, voor in het positie brengen en voor de return
+float Ki2 = 0.20;
+
+float Kp3 = 0.09; //Kp en Ki van motor1, voor de return
+float Ki3 = 0.05;
+
+volatile bool looptimerflag; //voor motorcontrol TSAMP
+
+//functies
+
+void setlooptimerflag(void)
+{
+ looptimerflag = true;
+
+}
+
+void keep_in_range(float * in, float min, float max)
+{
+*in > min ? *in < max? : *in = max: *in = max;
+}
+
+void looper()
+{
+ //put raw emg value of biceps and triceps in emg_biceps and emg_triceps, respectively
+ float emg_biceps; //Float voor EMG-waarde biceps
+ float emg_triceps; //Float voor EMG-waarde triceps
+
+ emg_biceps = emg0.read(); // read float value (0..1 = 0..3.3V) biceps
+ emg_triceps = emg1.read(); // read float value (0..1 = 0..3.3V) triceps
+
+ //process emg biceps
+ arm_biquad_cascade_df1_f32(&bihighpass, &emg_biceps, &emg_biceps, 1 );
+ arm_biquad_cascade_df1_f32(&binotch, &emg_biceps, &emg_biceps, 1 );
+ y0 = fabs(emg_biceps);
+ bi_result = (y0*0.1 +y1*0.1 + y2*0.1 + y3*0.1 + y4*0.1 + y5*0.1 + y6*0.1 + y7*0.1 + y8*0.1 + y9*0.1);
+ y9=y8;
+ y8=y7;
+ y7=y6;
+ y6=y5;
+ y5=y4;
+ y4=y3;
+ y3=y2;
+ y2=y1;
+ y1=y0;
+
+ //process emg triceps
+ arm_biquad_cascade_df1_f32(&trihighpass, &emg_triceps, &emg_triceps, 1 );
+ arm_biquad_cascade_df1_f32(&trinotch, &emg_triceps, &emg_triceps, 1 );
+ x0 = fabs(emg_triceps);
+ tri_result = (x0*0.1 +x1*0.1 + x2*0.1 + x3*0.1 + x4*0.1 + x5*0.1 + x6*0.1 + x7*0.1 + x8*0.1 + x9*0.1);
+ x9=x8;
+ x8=x7;
+ x7=x6;
+ x6=x5;
+ x5=x4;
+ x4=x3;
+ x3=x2;
+ x2=x1;
+ x1=x0;
+}
+
+void kalbi() //blinking three lights, first row - 2nd row unlit
+{
+ if(dir1==0) {
+ dir1 = dir2 = dir3 = 1;
+ } else {
+ dir1 = dir2 = dir3 = 0;
+ }
+}
+
+void kaltri() //blinking three lights, 2nd row - first row lit
+{
+ if(for1==0) {
+ for1 = for2 = for3 = 1;
+ } else {
+ for1 = for2 = for3 = 0;
+ }
+}
+
+void okay() //blinking the two lights you have chosen (misschien is hier een betere manier van coderen voor :P)
+{
+ if(direction == 1 && force == 1) { // links zwak
+ if(for1 == 0 && dir1 == 0) {
+ for1 = dir1 = 1;
+ } else {
+ for1 = dir1 = 0;
+ }
+ }
+ if(direction == 1 && force == 2) { // links normaal
+ if(for2 == 0 && dir1 == 0) {
+ for2 = dir1 = 1;
+ } else {
+ for2 = dir1 = 0;
+ }
+ }
+ if(direction == 1 && force == 3) { // links sterk
+ if(for3 == 0 && dir1 == 0) {
+ for3 = dir1 = 1;
+ } else {
+ for3 = dir1 = 0;
+ }
+ }
+ if(direction == 2 && force == 1) { // mid zwak
+ if(for1 == 0 && dir2 == 0) {
+ for1 = dir2 = 1;
+ } else {
+ for1 = dir2 = 0;
+ }
+ }
+ if(direction == 2 && force == 2) { // mid normaal
+ if(for2 == 0 && dir2 == 0) {
+ for2 = dir2 = 1;
+ } else {
+ for2 = dir2 = 0;
+ }
+ }
+ if(direction == 2 && force == 3) { // mid sterk
+ if(for3 == 0 && dir2 == 0) {
+ for3 = dir2 = 1;
+ } else {
+ for3 = dir2 = 0;
+ }
+ }
+ if(direction == 3 && force == 1) { // rechts zwak
+ if(for1 == 0 && dir3 == 0) {
+ for1 = dir3 = 1;
+ } else {
+ for1 = dir3 = 0;
+ }
+ }
+ if(direction == 3 && force == 2) { // rechts normaal
+ if(for2 == 0 && dir3 == 0) {
+ for2 = dir3 = 1;
+ } else {
+ for2 = dir3 = 0;
+ }
+ }
+ if(direction == 3 && force == 3) { // rechts sterk
+ if(for3 == 0 && dir3 == 0) {
+ for3 = dir3 = 1;
+ } else {
+ for3 = dir3 = 0;
+ }
+ }
+}
+
+int main()
+{
+ pc.baud(115200); //baudrate instellen
+ log_timer.attach(looper, TSAMP_EMG); //EMG, Fsample 500 Hz
+ looptimer.attach(setlooptimerflag,TSAMP);
+ pwm_motor1.period_us(100); //10kHz PWM frequency
+ pwm_motor2.period_us(100); //10kHz PWM frequency
+
+ //set up filters
+ arm_biquad_cascade_df1_init_f32(&binotch, 1, binotch_const, binotch_states);
+ arm_biquad_cascade_df1_init_f32(&bihighpass, 1, bihighpass_const, bihighpass_states);
+
+ arm_biquad_cascade_df1_init_f32(&trinotch, 1, trinotch_const, trinotch_states);
+ arm_biquad_cascade_df1_init_f32(&trihighpass, 1, trihighpass_const, trihighpass_states);
+
+ //kalibratie
+
+ //motorarm naar nul-positie
+ blink.attach(kalbi, 0.2);
+ blink.attach(kaltri, 0.2);
+
+ //calibration motor 2
+ pwm_motor2.write(0.6); //lage PWM
+ motor2dir = 1;
+ wait(1); // anders wordt de while(1) meteen onderbroken
+ while(1) {
+ if(motor2.getSpeed()*omrekenfactor2 > -0.70 && motor2.getSpeed()*omrekenfactor2 < 0.70) { // ik weet niet of het rechtsom of linksom zal gaan en misschien is de speed bij de gekozen pwm wel binnen dit bereik
+ pwm_motor2.write(0);
+ motor2.setPosition(0);
+ goto motor1cal;
+ }
+ wait(0.01);
+ }
+motor1cal:
+ //calibration motor 1
+ pwm_motor1.write(0.55); //lage PWM
+ motor1dir = 1;
+ wait(1); // anders wordt de while(1) meteen onderbroken
+ while(1) {
+ if(motor1.getSpeed()*omrekenfactor1 > -0.20 && motor1.getSpeed()*omrekenfactor1 < 0.20) { // ik weet niet of het rechtsom of linksom zal gaan en misschien is de speed bij de gekozen pwm wel binnen dit bereik
+ pwm_motor1.write(0);
+ motor1.setPosition(0);
+ goto emgcal;
+ }
+ wait(0.01);
+ }
+emgcal:
+ blink.detach();
+ dir1 = dir2 = dir3 = 1;
+ for1 = for2 = for3 = 1;
+ pc.printf("kalmoarm ");
+ wait (1);
+ for1 = for2 = for3 = 0;
+
+ //biceps kalibratie
+ blink.attach(kalbi, 0.2);
+ for (int kaltime = 0; kaltime<KALIBRATIONTIME; kaltime++) {
+ if (bi_max < bi_result) {
+ bi_max = bi_result;
+ }
+ wait (0.01);
+
+ blink.detach();
+ dir1 = dir2 = dir3 = 1;
+ pc.printf("kalbi ");
+ wait (1);
+
+ //triceps kalibratie
+ blink.attach(kaltri, 0.2);
+ for (int kaltime = 0; kaltime<KALIBRATIONTIME; kaltime++) {
+ if (tri_max < tri_result) {
+ tri_max = tri_result;
+ }
+ wait (0.01);
+ }
+ blink.detach();
+ for1 = for2 = for3 = 1;
+ pc.printf("kaltri ");
+ wait (1);
+ for1 = for2 = for3 = 0;
+ }
+
+directionchoice:
+log_timer.attach(looper, TSAMP_EMG);
+
+ while(1) { //Loop keuze DIRECTION
+ for(int i=1; i<4; i++) {
+ if(i==1) { //red
+ dir1=1;
+ dir2=0;
+ dir3=0;
+ for (int lag=0; lag<TIMEBETWEENBLINK; lag++) {
+ if(bi_result>0.8*bi_max) {
+ direction = 1;
+ pc.printf("links ");
+ wait(TIMEB4NEXTCHOICE); // Tijdelijke wait om cyaan lampje aan te zetten ter controle selectie
+ goto forcechoice; // goes to second while(1) for the deciding the force
+ } else {
+ wait(0.01);
+ }
+ }
+ }
+ if(i==2) { //green
+ dir1 =0;
+ dir2 =1;
+ dir3 =0;
+ for (int lag=0; lag<TIMEBETWEENBLINK; lag++) {
+ if(bi_result>0.8*bi_max) {
+ direction = 2;
+ pc.printf("mid ");
+ wait(TIMEB4NEXTCHOICE); // Tijdelijke wait om paars lampje aan te zetten ter controle selectie
+ goto forcechoice;
+ } else {
+ wait(0.01);
+ }
+ }
+ }
+ if(i==3) { //blue
+ dir1 =0;
+ dir2 =0;
+ dir3 =1;
+ for (int lag=0; lag<TIMEBETWEENBLINK; lag++) {
+ if(bi_result>0.8*bi_max) {
+ direction = 3;
+ pc.printf("rechts ");
+ wait(TIMEB4NEXTCHOICE); // Tijdelijke wait om oranje lampje aan te zetten ter controle selectie
+ goto forcechoice;
+ } else {
+ wait(0.01);
+ }
+ }
+ }
+ }
+ }
+forcechoice:
+ while(1) { //Loop keuze FORCE
+ for(int j=1; j<4; j++) {
+ if(j==1) { //red
+ for1=1;
+ for2=0;
+ for3=0;
+ for (int lag=0; lag<TIMEBETWEENBLINK; lag++) {
+ if(tri_result>0.8*tri_max) {
+ for1 = for2 = for3 = 0;
+ pc.printf("reset ");
+ goto directionchoice;
+ } else {
+ if(bi_result>0.8*bi_max) {
+ force = 1;
+ pc.printf("zwak ");
+ wait(TIMEB4NEXTCHOICE); // Tijdelijke wait om cyaan lampje aan te zetten ter controle selectie
+ goto choicesmade;
+ } else {
+ wait(0.01);
+ }
+ }
+ }
+ }
+ if(j==2) { //green
+ for1=0;
+ for2=1;
+ for3=0;
+ for (int lag=0; lag<TIMEBETWEENBLINK; lag++) {
+ if(tri_result>0.8*tri_max) {
+ for1 = for2 = for3 = 0;
+ pc.printf("reset ");
+ goto directionchoice;
+ } else {
+ if(bi_result>0.8*bi_max) {
+ force = 2;
+ pc.printf("normaal ");
+ wait(TIMEB4NEXTCHOICE); // Tijdelijke wait om paars lampje aan te zetten ter controle selectie
+ goto choicesmade;
+ } else {
+ wait(0.01);
+ }
+ }
+ }
+ }
+ if(j==3) { //blue
+ for1=0;
+ for2=0;
+ for3=1;
+ for (int lag=0; lag<TIMEBETWEENBLINK; lag++) {
+ if(tri_result>0.8*tri_max) {
+ for1 = for2 = for3 = 0;
+ pc.printf("reset ");
+ goto directionchoice;
+ } else {
+ if(bi_result>0.8*bi_max) {
+ force = 3;
+ pc.printf("sterk ");
+ wait(TIMEB4NEXTCHOICE); // Tijdelijke wait om oranje lampje aan te zetten ter controle selectie
+ goto choicesmade;
+ } else {
+ wait(0.01);
+ }
+ }
+ }
+ }
+ }
+ }
+
+choicesmade:
+ blink.attach(okay, 0.2);
+ while(1) {
+ if(tri_result>0.8*tri_max) {
+ blink.detach();
+ pc.printf("reset ");
+ wait(1); // 1 sec wait, anders reset je meteen ook de biceps keuze
+ goto forcechoice;
+ } else {
+ if(bi_result>0.8*bi_max && (dir1==1||dir2==1||dir3==1)) {
+ blink.detach();
+ log_timer.detach();
+ goto motorcontrol;
+ } else {
+ wait(0.01); // not sure of de wait noodzakelijk is (nu toegevoegd zodat het niet teveel strain levert op bordje)
+ }
+ }
+ }
+
+motorcontrol:
+
+ /* Vanaf hier komt de aansturing van de motor */
+
+ if(direction == 1 && force == 1) { // links zwak
+ pc.printf("links zwak ");
+ // hier komt setpoint motor 1, setpoint motor2
+ }
+ if(direction == 1 && force == 2) { // links normaal
+ pc.printf("links normaal ");
+ }
+ if(direction == 1 && force == 3) { // links sterk
+ pc.printf("links sterk ");
+ }
+ if(direction == 2 && force == 1) { // mid zwak
+ pc.printf("mid zwak ");
+ }
+ if(direction == 2 && force == 2) { // mid normaal
+ pc.printf("mid normaal ");
+ }
+ if(direction == 2 && force == 3) { // mid sterk
+ pc.printf("mid sterk ");
+ }
+ if(direction == 3 && force == 1) { // rechts zwak
+ pc.printf("rechts zwak ");
+ }
+ if(direction == 3 && force == 2) { // rechts normaal
+ pc.printf("rechts normaal ");
+ }
+ if(direction == 3 && force == 3) { // rechts sterk
+ pc.printf("rechts sterk ");
+ }
+
+ while(1) { // loop voor het goed plaatsen van motor2 (batje hoek)
+ while(!looptimerflag);
+ looptimerflag = false; //clear flag
+
+ //regelaar motor2, bepaalt positie
+ controlerror = setpoint2 - motor2.getPosition()*omrekenfactor2;
+ integral = integral + controlerror*TSAMP;
+ pwm = Kp2*controlerror + Ki2*integral;
+
+ keep_in_range(&pwm, -1,1);
+ pwm_motor2.write(abs(pwm));
+ if(pwm > 0) {
+ motor2dir = 1;
+ } else {
+ motor2dir = 0;
+ }
+
+ //controleert of batje positie heeft bepaald
+ if(batjeset < 200) { // dit is nog te bepalen, op dit moment als binnen marge van 1% voor 2 seconde, dan naar volgende motorcontrol
+ if (motor2.getPosition()*omrekenfactor2 > setpoint2*1.01 || motor2.getPosition()*omrekenfactor2 < setpoint2*0.99) {
+ batjeset = 0;
+ } else {
+ batjeset++;
+ }
+ } else {
+ pwm_motor2.write(0);
+ batjeset = integral = 0;
+ wait(1);
+ goto motor1control;
+ }
+ }
+
+motor1control:
+ while(1) { // loop voor het slaan mbv motor1 (batje snelheid)
+ while(!looptimerflag);
+ looptimerflag = false; //clear flag
+
+ if (balhit == 0) { //regelaar motor1, bepaalt snelheid
+ controlerror = setpoint1 - motor1.getSpeed()*omrekenfactor1;
+ integral = integral + controlerror*TSAMP;
+ pwm = Kp1*controlerror + Ki1*integral;
+ } else { //regelaar motor1, bepaalt positie
+ pwm_motor1.write(0);
+ balhit = integral = 0;
+ wait(1); // wait voordat arm weer naar beginpositie terugkeert
+ goto resetpositionmotor1;
+ }
+
+ keep_in_range(&pwm, -1,1);
+ pwm_motor1.write(abs(pwm));
+
+ if(pwm > 0) {
+ motor1dir = 1;
+ } else {
+ motor1dir = 0;
+ }
+
+ //controleert of batje balletje heeft bereikt
+ //if (motor1.getSpeed()*omrekenfactor1 >= 7.5 && motor1.getPosition()*omrekenfactor1 > 1.03 && motor1.getPosition()*omrekenfactor1 < 1.07) { vrij specifieke if-statement ter controle
+ if (motor1.getPosition()*omrekenfactor1 > 1.10) {
+ balhit = 1;
+ }
+ }
+
+resetpositionmotor1:
+ while(1) { // slagarm wordt weer in oorspronkelijke positie geplaatst
+ while(!looptimerflag);
+ looptimerflag = false; //clear flag
+
+ //regelaar motor1, bepaalt positie
+ controlerror = -1*motor1.getPosition()*omrekenfactor1;
+ integral = integral + controlerror*TSAMP;
+ pwm = Kp3*controlerror + Ki3*integral;
+
+ keep_in_range(&pwm, -1,1);
+ if(pwm > 0) {
+ motor1dir = 1;
+ } else {
+ motor1dir = 0; //1 = rechtsom, 0 = linksom
+ }
+
+ pwm_motor1.write(abs(pwm));
+
+ //controleert of arm terug in positie is
+ if(batjeset < 200) {
+ if (motor1.getPosition()*omrekenfactor1 > 0.03 || motor1.getPosition()*omrekenfactor1 < -0.03) {
+ batjeset = 0;
+ } else {
+ batjeset++;
+ }
+ } else {
+ pwm_motor1.write(0);
+ batjeset = integral = 0;
+ wait(1);
+ goto resetpositionmotor2;
+ }
+ }
+
+resetpositionmotor2:
+ while(1) { // loop voor het goed plaatsen van motor2 (batje hoek)
+ while(!looptimerflag);
+ looptimerflag = false; //clear flag
+
+ //regelaar motor2, bepaalt positie
+ controlerror = -1*motor2.getPosition()*omrekenfactor2;
+ integral = integral + controlerror*TSAMP;
+ pwm = Kp2*controlerror + Ki2*integral;
+
+ keep_in_range(&pwm, -1,1);
+
+ if(pwm > 0) {
+ motor2dir = 1;
+ } else {
+ motor2dir = 0;
+ }
+
+ pwm_motor2.write(abs(pwm));
+
+ //controleert of batje positie heeft bepaald
+ if(batjeset < 200) { // dit is nog te bepalen, op dit moment als binnen marge van 1% voor 2 seconde, dan naar volgende motorcontrol
+ if (motor2.getPosition()*omrekenfactor2 > 0.03 || motor2.getPosition()*omrekenfactor2 < -0.03) {
+ batjeset = 0;
+ } else {
+ batjeset++;
+ }
+ } else {
+ pwm_motor2.write(0);
+ batjeset = integral = 0;
+ wait(1);
+ direction = force = 0;
+ goto directionchoice;
+ }
+ }
+} // end main
\ No newline at end of file