Biorobotics_group_2
/
System_Identification
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: FastPWM HIDScope MODSERIAL QEI mbed
Fork of
Berekenen_motorhoek
by 
Biorobotics_group_2
Revision 7:167b6dfdefee, committed 2016-10-28
- Comitter:
- sjoerdbarts
- Date:
- Fri Oct 28 10:10:32 2016 +0000
- Parent:
- 6:ee243782bf51
- Commit message:
- Updated code
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Thu Oct 20 12:07:30 2016 +0000
+++ b/main.cpp Fri Oct 28 10:10:32 2016 +0000
@@ -24,7 +24,7 @@
DigitalOut led(LED_RED);
// Set HID scope
-HIDScope scope(2);
+HIDScope scope(3);
// Set Encoders for motors
QEI Motor1_ENC_CW(D11,D10,NC,32);
@@ -35,7 +35,7 @@
// Constants
volatile double pwm = 0.0;
volatile double pwm_new = 0;
-volatile double timer = 0.0;
+volatile double timer = 0.00000000;
volatile bool button1_value = false;
volatile bool button2_value = false;
@@ -64,6 +64,7 @@
int zero=0;
scope.set(0,zero);
scope.set(1,zero);
+ scope.set(2,zero);
scope.send();
}
@@ -95,18 +96,18 @@
{
if (MotorValue >=0)
{
- motor1_dir=1;
+ motor2_dir=1;
}
else
{
- motor1_dir=0;
+ motor2_dir=0;
}
if (fabs(MotorValue)>1){
- motor1_pwm.write(1);
+ motor2_pwm.write(1);
}
else
{
- motor1_pwm.write(abs(MotorValue));
+ motor2_pwm.write(abs(MotorValue));
}
}
@@ -136,30 +137,37 @@
void MultiSin_motor1()
{
- double pwm = pwm_new;
double f1 = 0.2f;
- double f2 = 1.0f;
- double f3 = 5.0f;
- double f4 = 20.0f;
- double f5 = 100.0f;
+ double f2 = 0.4f;
+ double f3 = 0.8f;
+ double f4 = 1.6f;
+ double f5 = 3.2f;
+ double f6 = 6.3f;
+ double f7 = 12.6f;
+ double f8 = 25.0f;
+ double f9 = 50.0f;
+ double f10 = 100.0f;
const double pi = 3.141592653589793238462;
// read encoder
double motor1_position = GetPosition(1);
// set HIDSCOPE values, position, pwm*volt
scope.set(0, motor1_position);
- scope.set(1, pwm);
- // sent HIDScope values
+ scope.set(1, pwm_new);
+ scope.set(2, timer);
scope.send();
+ // sent HIDScope values
+
// calculate the new multisin
- double multisin = sin(2*pi*f1*timer)+sin(2*pi*f2*timer)+sin(2*pi*f3*timer)+sin(2*pi*f4*timer)+sin(2*pi*f5*timer);
+ double multisin = sin(2*pi*f1*timer)+sin(2*pi*f2*timer)+sin(2*pi*f3*timer)+sin(2*pi*f4*timer)+sin(2*pi*f5*timer)+sin(2*pi*f6*timer)+sin(2*pi*f7*timer)+sin(2*pi*f8*timer)+sin(2*pi*f9*timer)+sin(2*pi*f10*timer);
// devide by the amount of sin waves
- pwm_new=multisin / 5;
+ pwm_new=multisin / 10.0f;
// write the motors
- SetMotor(1, multisin);
+ SetMotor(1, pwm_new);
// Increase the timer
- timer = timer + 0.001;
- if (timer >= (20.0-0.001))
+ timer = timer + 0.001f;
+ if (timer >= (10.00000f))
{
+ SetMotor(1, 0);
SinTicker.detach();
timer = 0;
}
@@ -167,30 +175,36 @@
void MultiSin_motor2()
{
- double pwm = pwm_new;
double f1 = 0.2f;
- double f2 = 1.0f;
- double f3 = 5.0f;
- double f4 = 20.0f;
- double f5 = 100.0f;
+ double f2 = 0.4f;
+ double f3 = 0.8f;
+ double f4 = 1.6f;
+ double f5 = 3.2f;
+ double f6 = 6.3f;
+ double f7 = 12.6f;
+ double f8 = 25.0f;
+ double f9 = 50.0f;
+ double f10 = 100.0f;
const double pi = 3.141592653589793238462;
// read encoder
double motor2_position = GetPosition(2);
// set HIDSCOPE values, position, pwm*volt
scope.set(0, motor2_position);
- scope.set(1, pwm);
- // sent HIDScope values
+ scope.set(1, pwm_new);
+ scope.set(2, timer);
scope.send();
+
// calculate the new multisin
- double multisin = sin(2*pi*f1*timer)+sin(2*pi*f2*timer)+sin(2*pi*f3*timer)+sin(2*pi*f4*timer)+sin(2*pi*f5*timer);
+ double multisin = sin(2*pi*f1*timer)+sin(2*pi*f2*timer)+sin(2*pi*f3*timer)+sin(2*pi*f4*timer)+sin(2*pi*f5*timer)+sin(2*pi*f6*timer)+sin(2*pi*f7*timer)+sin(2*pi*f8*timer)+sin(2*pi*f9*timer)+sin(2*pi*f10*timer);
// devide by the amount of sin waves
- pwm_new=multisin / 5;
- // write the motor
- SetMotor(2, multisin);
+ pwm_new=multisin / 10.0f;
+ // write the motors
+ SetMotor(2, pwm_new);
// Increase the timer
- timer = timer + 0.001;
- if (timer >= (20.0-0.001))
+ timer = timer + 0.001f;
+ if (timer >= (10.0000f))
{
+ SetMotor(2, 0);
SinTicker.detach();
timer = 0;
}
@@ -248,6 +262,8 @@
{
// just wait
}
+
+
pc.printf("\r\n ***************** \r\n");
pc.printf("\r\n Use potentiometers to control the motor arms \r\n");
pc.printf("\r\n Pot 1 for motor 1 \r\n");
@@ -262,8 +278,23 @@
{
// just wait
}
+ // When button is pressed detach PotControl and sendzeros to HIDScope
PotControlTicker.detach();
+ // Reset the motor PWM values to zero
+ SetMotor(1, 0);
+ SetMotor(2, 0);
+ // wait a bit to make sure all movement is gone
+ wait(0.5);
+ // Reset the QUE encoder values so that the currect position is nog the 0 position
+ Motor1_ENC_CW.reset();
+ Motor1_ENC_CCW.reset();
+ Motor2_ENC_CW.reset();
+ Motor2_ENC_CCW.reset();
+
+ // Flush the HIDScope with zeros
SendZerosTicker.attach(&SendZeros,0.001);
+ wait(1);
+ SendZerosTicker.detach();
// Program startup
pc.printf("\r\n Starting multisin on motor 1 in: \r\n");
@@ -282,17 +313,18 @@
pc.printf("\r\n 0 \r\n");
pc.printf("\r\n Wait for the signal to complete \r\n");
pc.printf("\r\n Press button 2 too continue afterwards \r\n");
- SendZerosTicker.detach();
SinControl1();
-
while (button2_value == true)
{
// just wait
}
-
+ // Flush the HIDScope with zeros
+ SendZerosTicker.attach(&SendZeros,0.001);
+ wait(1);
+ SendZerosTicker.detach();
- SendZerosTicker.attach(&SendZeros,0.001);
+ // Start multisine om motor 2 routine
pc.printf("\r\n Starting multisin on motor 2 in: \r\n");
pc.printf("\r\n 10 \r\n");
wait(5);
@@ -307,7 +339,6 @@
pc.printf("\r\n 1 \r\n");
wait(1);
pc.printf("\r\n 0 \r\n");
- SendZerosTicker.detach();
SinControl2();
wait(23);
pc.printf("\r\n Program Finished, press reset to restart \r\n");