Boudewijn van den Berg
v5
Dependencies: MODSERIAL TextLCD mbed
Revision 1:c18c171bf8b4, committed 2013-11-05
- Comitter:
- bouvdberg
- Date:
- Tue Nov 05 10:14:49 2013 +0000
- Parent:
- 0:0a171d2a8dc1
- Child:
- 2:b9a7fafd233c
- Commit message:
- V6
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Tue Nov 05 08:45:13 2013 +0000
+++ b/main.cpp Tue Nov 05 10:14:49 2013 +0000
@@ -26,14 +26,14 @@
#define LP1 0.9868
#define LP2 0.01325
//EMG threshold
-#define SET_EMG_MAX1 3.5 //bovenarm rechts > beweging naar rechts
-#define SET_EMG_MIN1 1.4
-#define SET_EMG_MAX2 3.6 //bovenarm links > beweging naar links
-#define SET_EMG_MIN2 1.4
-#define SET_EMG_MAX3 3.1 //onderarm rechts > beweging naar boven
-#define SET_EMG_MIN3 0.7
-#define SET_EMG_MAX4 6.6 //onderarm links > beweging naar onder
-#define SET_EMG_MIN4 1.0
+#define SET_EMG_MAX1 3.8 //bovenarm rechts > beweging naar rechts
+#define SET_EMG_MIN1 2.3
+#define SET_EMG_MAX2 3.8 //bovenarm links > beweging naar links
+#define SET_EMG_MIN2 0.7
+#define SET_EMG_MAX3 7.2 //onderarm rechts > beweging naar boven
+#define SET_EMG_MIN3 2.6
+#define SET_EMG_MAX4 1.8 //onderarm links > beweging naar onder
+#define SET_EMG_MIN4 0.8
void aansturing(void);
void uitzetten(void);
@@ -97,6 +97,7 @@
//float t_sin=0.0;
//float t_timer=0.0;
int sol_updown=0;
+ int t_sol=0;
int main() {
//set buttons PULLDOWN
@@ -138,11 +139,11 @@
motor2.setPosition(2400);
menu=55;
lcd.cls();
- lcd.printf(" Drawing ...");
- lcd.locate(0,1);
- lcd.printf("UP: Pause");
- //Solenoid=0;
- //sol_updown=0;
+ lcd.printf("UP: Pause");
+ lcd.locate(0,1);
+ lcd.printf("DOWN: Up/Down");
+ Solenoid=1;
+ sol_updown=0;
//wait(0.01);
}
}
@@ -340,10 +341,10 @@
lcd.locate(0,1);
lcd.printf(" ");
Solenoid=1;
- //sol_updown=1;?????????????????
+ sol_updown=1;
wait(1);
Solenoid=0;
- //sol_updown=0;?????????????????
+ sol_updown=0;
}
break;
case 50: //tekenen afsluiten
@@ -358,17 +359,22 @@
if (ButtonSTOP.read()==1) menu=50;
if (ButtonUP.read()==1) menu++;
- if (ButtonDOWN.read()==1) //Misschien in de loop van 'aansturing' zetten????????????????????
+ if (ButtonDOWN.read()==1) //Misschien in de loop van 'aansturing' zetten????????????????????
{
- if (sol_updown==1)
- {
- sol_updown=0;
- Solenoid=sol_updown;
- }
- if (sol_updown==0)
+ t_sol++;
+ if (t_sol>10)
{
- sol_updown=1;
- Solenoid=sol_updown;
+ if (sol_updown==0)
+ {
+ sol_updown=1;
+ Solenoid=1;
+ }
+ else
+ {
+ sol_updown=0;
+ Solenoid=0;
+ }
+ t_sol=0;
}
}
@@ -383,6 +389,7 @@
lcd.printf("> RESUME");
Solenoid=1;
+ sol_updown=1;
pwm_motor1.write(0);
pwm_motor2.write(0);
@@ -740,14 +747,18 @@
//pc.printf("%.2f ",v2);
//pc.printf("%.2f ",v3);
//pc.printf("%.2f ",v4);
- if(v1<=0.1 && v2<=0.1 && v3<=0.1 && v4<=0.1) {
+ if(v1<=0.0 && v2<=0.0 && v3<=0.0 && v4<=0.0) {
Solenoid=1; //Pen van papier
input=0.0;
snelheid=0.0;
}
else {
if (sol_updown==1) Solenoid=1; //Pen op papier
- else Solenoid=0;
+ else
+ {
+ Solenoid=0;
+ sol_updown=0;
+ }
snelheid=drawspeed; //Is dit goed zo????????????????????????????????????????????????????????? Haakjes nodig????
if(v2>v1) {
input=(atan((v3-v4)/(v1-v2))+PI);
@@ -815,12 +826,12 @@
//Motoraansturing
//t_sin=t_sin + 0.05;
//if (t_sin>=2*PI) t_sin=0.0;
- setpointM1 = (w1/(2.0*PI))*3200.0*LOOPTIME+setpointmin1M1; //sin(t_sin)*1600;
+ setpointM1 = (w1/(2.0*PI))*3200.0*LOOPTIME+setpointmin1M1;
setpointM2 = (wM2/(2.0*PI))*3200.0*LOOPTIME+setpointmin1M2;
//Beperking hoeken
- keep_in_range(&setpointM1, 500,1400);//Heel rondje = 3200 pulsen, Half rondje = 1600 pulsen
- keep_in_range(&setpointM2, 1600,2950);// Begrensd op 20 graden minimaal, werkelijke minimale waarde is 15 graden
+ keep_in_range(&setpointM1, 500,1400); //Heel rondje = 3200 pulsen, Half rondje = 1600 pulsen
+ keep_in_range(&setpointM2, 1600,2950); // Begrenzing hoeken
foutM1 = setpointM1-M1position;
foutM2 = setpointM2-M2position;
@@ -838,11 +849,11 @@
if(pwm_to_motor1 > 0) {
motordir1 = 1;
- pwm_to_motor1=pwm_to_motor1+0.05;
+ pwm_to_motor1=pwm_to_motor1+0.03;
}
else {
motordir1 = 0;
- pwm_to_motor1=pwm_to_motor1-0.05;
+ pwm_to_motor1=pwm_to_motor1-0.03;
}
if(pwm_to_motor2 > 0) {
motordir2 = 1;
@@ -855,8 +866,12 @@
//WRITE VALUE TO MOTOR
- //if(v1<=0.1 && v2<=0.1 && v3<=0.1 && v4<=0.1)
+ //if(v1<=0.01 && v2<=0.01 && v3<=0.01 && v4<=0.01)
//{
+ // motordir1 = 1;
+ // motordir2 = 1;
+ // Brake1=1;
+ // Brake2=1;
// pwm_motor1.write(0);
// pwm_motor2.write(0); //motor stil
//}
@@ -894,8 +909,9 @@
{
float BeginM1 = 800;
float BeginM2 = 2400;
- int i_timer=800;
+ int i_timer=500;
Solenoid=1;
+ sol_updown=1;
while(BeginM1 - motor1.getPosition() >= 10 || BeginM1 - motor1.getPosition() <= -10 || BeginM2 - motor2.getPosition() >= 10 || BeginM2 - motor2.getPosition() <= -10 || motor1.getSpeed()>=20 || motor2.getSpeed()>=20)
{
Ticker looptimer;
@@ -925,16 +941,16 @@
i_timer--;
}
+ motordir1 = 1;
+ motordir2 = 1;
+ Brake1=1;
+ Brake2=1;
pwm_motor1.write(0);
pwm_motor2.write(0);
- motordir1 = 0;
- motordir2 = 0;
- Brake1=1;
- Brake2=1;
wait(1.0);
Brake1=0;
Brake2=0;
- i_timer=500;
+ i_timer=300;
while(BeginM1 - motor1.getPosition() >= 10 || BeginM1 - motor1.getPosition() <= -10 || BeginM2 - motor2.getPosition() >= 10 || BeginM2 - motor2.getPosition() <= -10 || motor1.getSpeed()>=20 || motor2.getSpeed()>=20)
{
Ticker looptimer;
@@ -964,19 +980,19 @@
i_timer--;
}
+ motordir1 = 1;
+ motordir2 = 1;
+ Brake1=1;
+ Brake2=1;
pwm_motor1.write(0);
pwm_motor2.write(0);
- motordir1 = 0;
- motordir2 = 0;
- Brake1=1;
- Brake2=1;
wait(1.0);
Brake1=0;
Brake2=0;
setpointM1=800.0;
setpointM2=2400.0;
- //setpointmin1M1=800.0;
- //setpointmin1M2=2400.0;
+ setpointmin1M1=800.0;
+ setpointmin1M2=2400.0;
pwm_to_motor1=0.0;
pwm_to_motor2=0.0;
foutM1=0.0;