Peter Bartels
Laatste versie van ons script
Dependencies: Encoder HIDScope MODSERIAL TextLCD mbed-dsp mbed
Fork of
Main-script_groep7_V3
by 
Peter Bartels
Diff: main.cpp
- Revision:
- 8:f733c6a27c15
- Parent:
- 7:7e3e183bf063
- Child:
- 9:7e9b63fe8988
--- a/main.cpp Fri Oct 31 17:14:30 2014 +0000
+++ b/main.cpp Sat Nov 01 13:20:22 2014 +0000
@@ -10,9 +10,6 @@
/* -Peter Bartels */
/********************************************/
-/*
-#include voor alle libraries
-*/
#include "TextLCD.h"
#include "mbed.h"
#include "encoder.h"
@@ -20,23 +17,16 @@
#include "PwmOut.h"
#include "arm_math.h"
-/*
-#define vaste waarden
-*/
-/*definieren pinnen Motor 1*/
+/*definieren pinnen Motoren*/
#define M1_PWM PTA5
#define M1_DIR PTA4
#define M2_PWM PTC8
#define M2_DIR PTC9
-/*Definieren minimale waarden PWM per motor*/
-#define PWM1_min_50 0.529 /*met koppelstuk!*/
-#define PWM2_min_30 0.529 /*aanpassen! Klopt nog niet met koppelstuk*/
/*Definieren om de hoeveel seconden er gegevens naar de HID-scope gestuurd worden.*/
#define TSAMP 0.005
-#define K_P (5)
-#define K_I (0.1 * TSAMP)
-#define K_D (0)
-//#define K_D (0.0005 /TSAMP)
+#define K_P (80000)
+#define K_I (0.01)
+#define K_D (0.01)
#define I_LIMIT 100.
#define PI 3.1415926535897
#define lengte_arm 0.5
@@ -55,7 +45,7 @@
DigitalOut LEDRED(LED_RED);
Serial pc(USBTX,USBRX);
HIDScope scope(3);
-AnalogIn emg(PTB1);
+AnalogIn emg(PTB1);
/*
definieer namen aan var, float, int, static float, uint8_t, uint16_t etc. en geef ze eventueel een waarde
*/
@@ -67,38 +57,20 @@
arm_biquad_casd_df1_inst_f32 notch; //2e orde lowpass biquad butterworthfilter 50Hz
int previous_herhalingen = 0;
int current_herhalingen = 0;
-int PWM2_percentage = 100;
-int aantal_rotaties_1 = 10;
-int aantalcr_1 = 1600;
-int aantalcr_2 = 960;
-int beginpos_motor1;
-int beginpos_motor1_new;
-int beginpos_motor2;
-int beginpos_motor2_new;
int previous_pos_motor1 = 0;
int current_pos_motor1;
int EMG = 1;
-int delta_pos_motor1_puls;
int aantal_pieken;
int doel;
int doel_richting;
int doel_hoogte;
bool aanspan;
void clamp(float * in, float min, float max);
-volatile bool looptimerflag;
-int16_t gewenste_snelheid = 2;
-int16_t gewenste_snelheid_rad = 4;
-int16_t gewenste_snelheid_rad_return = 1;
float pid(float setpoint, float measurement);
float pos_motor1_rad;
float PWM1_percentage = 0;
-float delta_pos_motor1_rad;
-float snelheid_motor1_rad;
-float snelheid_arm_ms;
float PWM1;
float PWM2;
-float Speed_motor1;
-float Speed_motor1rad;
float setpoint = 0;
float prev_setpoint = 0;
float lowpass_1_const[] = {0.978030479206560 , 1.956060958413119 , 0.978030479206560 , -1.955578240315036 , -0.956543676511203};
@@ -114,28 +86,25 @@
float emg_treshhold_laag = 0;
float emg_treshhold_hoog = 0;
-//HIDScope scope(6);
-
enum state_t {RUST, EMG_KALIBRATIE, ARM_KALIBRATIE, METEN_HOOGTE, METEN_RICHTING, INSTELLEN_RICHTING, SLAAN, RETURN2RUST}; //verschillende stadia definieren voor gebruik in CASES
state_t state = RUST;
-//functies die vanuit de statemachinefunction aangeroepen kunnen worden
void rust() {
current_herhalingen = previous_herhalingen + 1; previous_herhalingen = current_herhalingen;
-}
+}//void rust
void pieken_tellen(){
if (emg_filtered>=emg_treshhold_hoog)
{
aanspan=true; //maak een variabele waarin je opslaat dat het signaal hoog is.
- }
+ }//if
if (aanspan==true && emg_filtered<=emg_treshhold_laag)//== ipv =, anders wordt aanspan true gemaakt
{
aanspan=false;
aantal_pieken++;
doel = aantal_pieken-((aantal_pieken/3)*3)+1;
- }
-}
+ }//if
+}//void pieken_tellen
void emg_filtering() {
uint16_t emg_value;
@@ -155,18 +124,18 @@
if(state!=EMG_KALIBRATIE)
{
pieken_tellen();
- }
-}
+ }//if
+}//void emg_filtering()
void emg_max_meting(){
emg_filtering();
if (emg_filtered>=emg_max)
{
emg_max=emg_filtered;
- }
+ }//if
emg_treshhold_laag = 0.3*emg_max;
emg_treshhold_hoog = 0.7*emg_max;
-}
+}//void emg_max_meting
void emg_kalibratie() {
//if(emg_filtered>=0.05){//Deze if-loop alleen zodat het nog op de hidscope kan worden gezien, dit mag weg wanneer er een display is, current_herhalingen wel laten.
@@ -175,56 +144,51 @@
if(current_herhalingen<=1000)
{
emg_max_meting();
- }
-}
+ }//if
+}//void emg_kalibratie
void arm_kalibratie() {
//voor nu om de loop te doorlopen wordt onderstaande code gebruikt. Nogmaal gesproken moet er gewacht worden op een 'hoog' signaal van een knop
current_herhalingen = previous_herhalingen + 1; previous_herhalingen = current_herhalingen;
motor1.setPosition(0);
motor2.setPosition(0);
-}
+}//void arm_kalibratie
void doel_bepaling() {
if(200<=current_herhalingen<=1200){
emg_filtering();
- }
+ }//if
else if(1600<=current_herhalingen<=2200){
emg_filtering();
- }
+ }//else if
else{
- }
-}
+ }//else
+}//void doel_bepaling
void meten_hoogte() {
current_herhalingen = previous_herhalingen + 1; previous_herhalingen = current_herhalingen;
doel_bepaling();
if(1600<=current_herhalingen<=2200 && aantal_pieken==1){
doel=doel_hoogte;
- }
-}
+ }//if
+}//void meten_hoogte
void meten_richting() {
current_herhalingen = previous_herhalingen + 1; previous_herhalingen = current_herhalingen;
doel_bepaling();
if(1600<=current_herhalingen<=2200 && aantal_pieken==1){
doel=doel_richting;
- }
-}
+ }//if
+}//void meten_richting
void instellen_richting() {
current_herhalingen = previous_herhalingen + 1; previous_herhalingen = current_herhalingen;
-}
+}//void instellen_richting
void GotoPosition (float position_setpoint_rad, float speed_radpersecond) {
current_pos_motor1 = motor1.getPosition(); //bekijk na elke 0.005s wat de huidige 'waarde' van de encoder is
- //delta_pos_motor1_puls = current_pos_motor1 - previous_pos_motor1; //Bereken wat het verschil in waarde is voor het berekenen van de snelheid(puls)
pos_motor1_rad = current_pos_motor1/(1600/(2*PI)); //echte waarde omrekenen naar rad voor (positie) PID regelaar
- //delta_pos_motor1_rad = delta_pos_motor1_puls/(1600/(2*PI)); //delta waarde omrekenen naar rad voor snelheidsbepaling (rad)
- //snelheid_motor1_rad = delta_pos_motor1_rad / TSAMP; //rad delen door TSAMP om rad/s te verkrijgen
- //snelheid_arm_ms = snelheid_motor1_rad * lengte_arm; //deze waarde maar lengte van de arm om de snelheid van het uiteinde van die arm te verkrijgen
- //scope.set(0, snelheid_motor1_rad);
previous_pos_motor1 = current_pos_motor1; //sla de huidige waarde op als vorige waarde.
@@ -238,46 +202,40 @@
previous_herhalingen = current_herhalingen;
pc.printf("stop\n\r");
PWM1_percentage = 0;
- }
+ }//if
else if(pos_motor1_rad - position_setpoint_rad < 0)
{
setpoint = prev_setpoint +( TSAMP * speed_radpersecond);
PWM1_percentage = pid(setpoint, pos_motor1_rad);
- }
+ }//else if
else
{
setpoint = prev_setpoint -( TSAMP * speed_radpersecond);
PWM1_percentage = pid(setpoint, pos_motor1_rad);
- }
- /*new_pwm = (setpoint - motor1.getPosition())*.001; -> P action*/
+ }//else
pc.printf("%f\n\r",PWM1_percentage);
-
- //scope.set(1, setpoint-pos_motor1_rad);
-
if (PWM1_percentage < -100)
{
PWM1_percentage = -100;
- }
+ }//if
else if (PWM1_percentage >100)
{
PWM1_percentage =100;
- }
+ }//else if
if(PWM1_percentage < 0)
{
motordir1 = 1;
- }
+ }//if
else
{
motordir1 = 0;
- }
+ }//else
- pwm_motor1.write(abs(PWM1_percentage/100.));//abs(((1-PWM1_min_50)/100)*PWM1_percentage + PWM1_min_50));
- //scope.set(5, abs(((1-PWM1_min_50)/100)*PWM1_percentage + PWM1_min_50));
+ pwm_motor1.write(abs(PWM1_percentage/100.))
prev_setpoint = setpoint;
- //scope.send();
-}
+}//void GotoPosition
float pid(float setpoint, float measurement)
{
@@ -292,18 +250,13 @@
out_d = (error-prev_error)*K_D;
clamp(&out_i,-I_LIMIT,I_LIMIT);
prev_error = error;
- //scope.set(2, out_p);
- //scope.set(3, out_i);
- //scope.set(4, out_d);
return out_p + out_i + out_d;
-}
+}//float pid
-void clamp(float* in, float min, float max) // "*" is een pointer (verwijst naar het adres waar een variebele instaat). Dus je slaat niet de variabele op
-// maar de locatie van de variabele.
+void clamp(float* in, float min, float max)
{
- *in > min ? /*(*/*in < max? /*niets doen*/ : *in = max/*)*/: *in = min; // a ? b : c --> als a waar is, dan doe je b, en anders c
- // *in = het getal dat staat op locatie van in --> waarde van new_pwm
-}
+ *in > min ? /*(*/*in < max? /*niets doen*/ : *in = max/*)*/: *in = min;
+}//void clamp
void statemachinefunction()
{
@@ -317,16 +270,9 @@
previous_herhalingen = 0;
state = ARM_KALIBRATIE;
EMG = 0;
- }
+ }//if (current_herhalingen == 100 && EMG == 1)
break;
- /*bepalen hoe je zorgt dat je maar 1x de kalibraties uitvoerd! (Of eventueel de arm kalibratie elke keer!!)*/
- //if (metingstatus<5);
- // state = ARMKALIBRATIE;
- //if (metingstatus==5);
- // state = METEN_RICHTING;
- //break;
- //}
- }
+ }//case RUST:
case ARM_KALIBRATIE:
{
@@ -340,9 +286,9 @@
motor2.setPosition(0);
pwm_motor1.period_us(100);
pwm_motor2.period_us(100);
- }
+ }//if (current_herhalingen == 100)
break;
- }
+ }//case ARM_KALIBRATIE:
case EMG_KALIBRATIE:
{
@@ -354,9 +300,9 @@
previous_herhalingen = 0;
aantal_pieken = 0;
doel = 0;
- }
+ }//if (current_herhalingen >=1000)
break;
- }
+ }//case EMG_KALIBRATIE
case METEN_HOOGTE:
{
@@ -368,7 +314,7 @@
previous_herhalingen = 0;
aantal_pieken = 0;
doel = 0;
- }
+ }//if (current_herhalingen == 2800 && aantal_pieken == 1)
else
{
state = METEN_HOOGTE;
@@ -376,9 +322,9 @@
previous_herhalingen = 0;
aantal_pieken = 0;
doel = 0;
- }
+ }///else
break;
- }
+ }//case METEN_HOOGTE
case METEN_RICHTING:
{
@@ -390,7 +336,7 @@
previous_herhalingen = 0;
aantal_pieken = 0;
doel = 0;
- }
+ }//if (current_herhalingen == 2800 && aantal_pieken == 1)
else
{
state = METEN_RICHTING;
@@ -398,9 +344,9 @@
previous_herhalingen = 0;
aantal_pieken = 0;
doel = 0;
- }
+ }//else
break;
- }
+ }//case METEN_RICHTING
case INSTELLEN_RICHTING:
{
@@ -410,14 +356,13 @@
state = SLAAN;
current_herhalingen = 0;
previous_herhalingen = 0;
- }
- break;
-
- }
+ }//if (current_herhalingen == 100)
+ break;
+ }//case INSTELLEN_RICHTING
case SLAAN:
{
- GotoPosition(1.5 ,8);
+ GotoPosition(1.5 ,8, 0.1);
if (current_herhalingen == 100)
{
state = RETURN2RUST;
@@ -425,27 +370,25 @@
previous_herhalingen = 0;
prev_setpoint =0;
setpoint =0;
-
- }
+ }//if (current_herhalingen == 100)
break;
- }
+ }//case SLAAN
case RETURN2RUST:
{
- GotoPosition(0,2);
- if (current_herhalingen == 100)
- {
- state = RUST;
- current_herhalingen = 0;
- previous_herhalingen = 0;
- }
-
+ GotoPosition(0,4, 0.05);
+ //if (current_herhalingen == 100)
+ //{
+ // state = RUST;
+ // current_herhalingen = 0;
+ // previous_herhalingen = 0;
+ //}//if (current_herhalingen == 100)
break;
- }
+ }// case RETURN2RUST
default: {
state = RUST;
- }
+ }//default
}//switch(state)
}//void statemachinefunction
@@ -458,7 +401,8 @@
lcd.printf("V.I.C.T.O.R.Y."); //regel 1 LCD scherm
lcd.locate(0,1);
lcd.printf(" GROEP 7 ");
- }
+ }//if(state==RUST)
+
else if(state==EMG_KALIBRATIE){
lcd.cls();
lcd.locate(0,0);
@@ -466,24 +410,25 @@
if(current_herhalingen<=200){
lcd.locate(0,1);
lcd.printf("nog 5 sec.");
- }
+ }//if(current_herhalingen<=200)
else if(current_herhalingen<=400){
lcd.locate(0,1);
lcd.printf("nog 4 sec.");
- }
+ }//else if(current_herhalingen<=400)
else if(current_herhalingen<=600){
lcd.locate(0,1);
lcd.printf("nog 3 sec.");
- }
+ }//else if(current_herhalingen<=600)
else if(current_herhalingen<=800){
lcd.locate(0,1);
lcd.printf("nog 2 sec.");
- }
+ }//else if(current_herhalingen<=800)
else if(current_herhalingen<=1000){
lcd.locate(0,1);
lcd.printf("nog 1 sec.");
- }
- }
+ }//else if(current_herhalingen<=1000)
+ }//else if(state==EMG_KALIBRATIE)
+
else if(state==METEN_HOOGTE){
lcd.cls();
if(current_herhalingen<=200){
@@ -491,48 +436,49 @@
lcd.printf("Hoogte bepalen:");
lcd.locate(0,1);
lcd.printf("span aan per vak");
- }
+ }//if(current_herhalingen<=200){
else if(200<=current_herhalingen<=1200){
lcd.locate(0,0);
lcd.printf("Vak %d",doel);
if(current_herhalingen<=400){
lcd.locate(0,1);
lcd.printf("nog 5 sec.");
- }
+ }//if(current_herhalingen<=400)
else if(current_herhalingen<=600){
lcd.locate(0,1);
lcd.printf("nog 4 sec.");
- }
+ }//else if(current_herhalingen<=600)
else if(current_herhalingen<=800){
lcd.locate(0,1);
lcd.printf("nog 3 sec.");
- }
+ }//else if(current_herhalingen<=800)
else if(current_herhalingen<=1000){
lcd.locate(0,1);
lcd.printf("nog 2 sec.");
- }
+ }//else if(current_herhalingen<=1000)
else if(current_herhalingen<=1200){
lcd.locate(0,1);
lcd.printf("nog 1 sec.");
- }
- }
+ }//else if(current_herhalingen<=1200)
+ }//else if(200<=current_herhalingen<=1200)
else if(current_herhalingen<=1600){
lcd.locate(0,0);
lcd.printf("Vak %d akkoord?",doel);
lcd.locate(0,1);
lcd.printf("Span aan");
- }
+ }//else if(current_herhalingen<=1600){
else if(1600<=current_herhalingen<=2800 && aantal_pieken==1){
lcd.locate(0,0);
lcd.printf("Vak %d akkoord",doel_hoogte);
- }
+ }//else if(1600<=current_herhalingen<=2800 && aantal_pieken==1){
else{
lcd.locate(0,0);
lcd.printf("Opnieuw hoogte");
lcd.locate(0,1);
lcd.printf("bepalen");
- }
- }
+ }//else{
+ }//else if(state==METEN_HOOGTE){
+
else if(state==METEN_RICHTING){
lcd.cls();
if(current_herhalingen<=200){
@@ -540,52 +486,53 @@
lcd.printf("Richting bepalen:");
lcd.locate(0,1);
lcd.printf("span aan per vak");
- }
+ }//if(current_herhalingen<=200)
else if(200<=current_herhalingen<=1200){
lcd.locate(0,0);
lcd.printf("Vak %d",doel);
if(current_herhalingen<=400){
lcd.locate(0,1);
lcd.printf("nog 5 sec.");
- }
+ }//if(current_herhalingen<=400)
else if(current_herhalingen<=600){
lcd.locate(0,1);
lcd.printf("nog 4 sec.");
- }
+ }//else if(current_herhalingen<=600)
else if(current_herhalingen<=800){
lcd.locate(0,1);
lcd.printf("nog 3 sec.");
- }
+ }//else if(current_herhalingen<=800)
else if(current_herhalingen<=1000){
lcd.locate(0,1);
lcd.printf("nog 2 sec.");
- }
+ }//else if(current_herhalingen<=1000)
else if(current_herhalingen<=1200){
lcd.locate(0,1);
lcd.printf("nog 1 sec.");
- }
- }
+ }//else if(current_herhalingen<=1200)
+ }//else if(200<=current_herhalingen<=1200)
else if(current_herhalingen<=1600){
lcd.locate(0,0);
lcd.printf("Vak %d akkoord?",doel);
lcd.locate(0,1);
lcd.printf("Span aan");
- }
+ }//else if(current_herhalingen<=1600)
else if(1600<=current_herhalingen<=2800 && aantal_pieken==1){
lcd.locate(0,0);
lcd.printf("Vak %d akkoord",doel_richting);
- }
+ }//else if(1600<=current_herhalingen<=2800 && aantal_pieken==1)
else{
lcd.locate(0,0);
lcd.printf("Opnieuw richting");
lcd.locate(0,1);
lcd.printf("bepalen");
- }
- }
+ }//else
+ }//else if(state==METEN_RICHTING){
+
else{
lcd.cls();
lcd.printf("state %d", state); //hier nog aan toevoegen hoe je de 'waarde', dus eigenlijk tekst, die opgeslagen staat in state kan printen.
- }
+ }//else{
}
int main(){