Fabian van Hummel
gebruik dit voor potmeter en shit
Dependencies: HIDScope MODSERIAL PID QEI biquadFilter mbed
Revision 2:4baa95d7bc5f, committed 2016-11-02
- Comitter:
- fabian101
- Date:
- Wed Nov 02 15:42:16 2016 +0000
- Parent:
- 1:c155f3c67deb
- Commit message:
- TEST_PROGRAM
Changed in this revision
--- a/Encoder.lib Mon Oct 10 16:14:59 2016 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,1 +0,0 @@ -http://developer.mbed.org/users/vsluiter/code/Encoder/#18b000b443af
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/HIDScope.lib Wed Nov 02 15:42:16 2016 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/tomlankhorst/code/HIDScope/#188304906687
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/PID.lib Wed Nov 02 15:42:16 2016 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/aberk/code/PID/#6e12a3e5af19
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/QEI.lib Wed Nov 02 15:42:16 2016 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/aberk/code/QEI/#5c2ad81551aa
--- a/main.cpp Mon Oct 10 16:14:59 2016 +0000
+++ b/main.cpp Wed Nov 02 15:42:16 2016 +0000
@@ -1,76 +1,80 @@
#include "mbed.h"
#include "MODSERIAL.h"
+//#include "biquadFilter.h"
#include "math.h"
-#include "Encoder.h"
+#include "HIDScope.h"
+#include "QEI.h"
// Occupied pins A1,D2,D4,D5,D12,D13
-AnalogIn potMeter1(A1);
DigitalOut motor1_dir(D4);
PwmOut motor1_mag(D5);
-DigitalIn button1(D2);
-Encoder motor1(D13,D12);
-AnalogIn setpoint_1(A0);
-
-//PwmOut led(D9);
-//HIDScope scope(3);
+QEI motor1_pos(D13 ,D12 ,NC, 64 ,QEI::X4_ENCODING); // D12 on A D13 on B
+
+DigitalIn button_1(D2);
+DigitalIn button_2(D3);
+AnalogIn potMeter_1(A1);
MODSERIAL pc(USBTX, USBRX);
float getReferenceVelocity();
-float FeedForwardControl(float);
-void SetMotor1(float);
+float feedforwardControl(float);
+void setMotor_1(float);
bool getDirection(bool);
+float pulsesToDegrees(int);
+void test_hid_ticker();
+void initpositionGo();
const float maxVelocity= 8.4; // should be 8.4 for 80 RPM
-const float MotorGain=8.4; // unit: (rad/s)
+const float motorGain= 8.4; // unit: (rad/s)
const int CW = 1;
const int CCW = 0;
-bool direction = 1;
-motor1_mag.period(1.0/10000); //10kHz
-/*
- float degrees_setpoint;
- float error;
- float output;
- const float degrees_per_count = 90/200.0; //quick approach
- const float ain_degrees_range = 90; //degrees over which the potmeter can control.
- const float kp_1_range = -1;
-*/
+bool direction = 1;
+
int main()
{
- while (true) {
- float ReferenceVelocity = getReferenceVelocity(); // returns direction + speed in rad/s
- float motorValue = FeedForwardControl(ReferenceVelocity); // returns value between -1 and 1
- SetMotor1(motorValue); // adjust motor
- wait(2);
+ while (button_2) {
+
+ float ReferenceVelocity = getReferenceVelocity(); // returns direction + speed in rad/s
+ float motorValue = feedforwardControl(ReferenceVelocity); // returns value between -1 and 1
+ setMotor_1(motorValue); // adjust motor
+ //float motor1_deg = pulsesToDegrees(motor1_pos.getPulses()) % 360;
+ //pc.printf("motor1_deg: %f \n", motor1_deg);
+ int pulses = motor1_pos.getPulses();
+ pc.printf("Pulses: %d\n\r", pulses);
+ //float revolutions = float(pulses/8400.00); // 8384 or 8400, output shaft revolutions
+ //pc.printf("Revolutions: %f\n", revolutions);
+ wait(0.002);
}
+motor1_mag.write(0);
}
float getReferenceVelocity() // Returns reference velocity in rad/s.
{
float referenceVelocity; // in rad/s
- float potmeter1 = potMeter1; // read out potmeter
- // pc.printf("%f\n", potmeter1);
- if (getDirection(direction) == CW) { // Clockwise rotation
+ float potmeter1 = potMeter_1.read(); // read out potmeter
+ pc.printf("%f\n", potmeter1);
+ if (getDirection(direction) == CW) { // Clockwise rotatio
referenceVelocity = potmeter1 * maxVelocity;
}
else { // Counterclockwise rotation
- referenceVelocity = -1*potMeter1 * maxVelocity;
+ referenceVelocity = -1*potmeter1 * maxVelocity;
}
pc.printf("referenceVelocity: %f\n", referenceVelocity);
return referenceVelocity; // duty cycle compensated
}
-float FeedForwardControl(float referenceVelocity)
+float feedforwardControl(float referenceVelocity)
{
- float motorValue = referenceVelocity / MotorGain;
+ float motorValue = referenceVelocity / motorGain;
pc.baud(115200);
- pc.printf("motorValue: %f\n", motorValue);
+ pc.printf("motorValue: %f\n\r", motorValue);
return motorValue; // between -1 and 1
}
-void SetMotor1(float motorValue) // this sets the PWM and direction
+void setMotor_1(float motorValue) // this sets the PWM and direction
{
+
if (motorValue >=0){
motor1_dir.write(CW);
}
@@ -78,7 +82,7 @@
motor1_dir.write(CCW);
}
int motor1_Dir = motor1_dir;
- pc.printf("motor1_dir: %d\n", motor1_Dir);
+ pc.printf("motor1_dir: %d\n\r", motor1_Dir);
if (fabs(motorValue)>1) {
motor1_mag.write(1); // safety, magnitude max 1
@@ -87,40 +91,46 @@
motor1_mag.write(fabs(motorValue)); //PWM Speed Control
}
float motor1_Mag = motor1_mag;
- pc.printf("motor1_mag: %f\n", motor1_Mag);
- float sensorMag = motor1_mag.getPosition();
+ pc.printf("motor1_mag: %f\n\r", motor1_Mag);
}
bool getDirection(bool Direction){
- direction = Direction;
- if (button1){
- direction = !direction;
+ if (!button_1){ // pressed
+
+ return 1;
+
}
- return direction;
+ else { return 0;}
}
- /*
-int main()
-{
+
+// not pressed is positive for long side SO going RIGHT, so DIR is 1
+// presssed is negative for long side so going left, so DIR is 0
+// not pressed is positive for short side so going up, so DIR is 1
+// pressed is negative for short side so going down, so DIR is 0
+
+/*float pulsesToLinpos(int pulses) {
+ float Linpos = ((pulses)/(X*N))*(1/PPCM );
+ return Linpos; // in Centimeters
+}
+*/
- while (true) {
- degrees_setpoint = setpoint_1 * ain_degrees_range;
- error = degrees_setpoint-(motor1.getPosition()*degrees_per_count); //error = setpoint - current
- output = error*p_value*kp_1_range;
- if(output > 0)
- {
- direction_1.write(true);
+/*float revToDegrees(float revolutions){
+ double degrees = revolutions*360;
+}
+*/
+
+void initpositionGo(){
+bool initposition = false;
+ while (initposition){
+ float pulseCount = motor1_pos.getPulses();
+ if ( pulseCount < 0) {
+ motor1_dir.write(CW);
}
- else
- {
- direction_1.write(false);
+ else if (pulseCount > 0){
+ motor1_dir.write(CCW);
}
- pwm_1.write(fabs(output));
- scope.set(0,motor1.getPosition());
- scope.set(1,error);
- scope.set(2,fabs(output));
- led.write(motor1.getPosition()/100.0);
- scope.send();
- wait(0.01);
+ else {
+ initposition = true;
+ }
}
}
-*/
\ No newline at end of file