Revision 0:35f03aa18046, committed 2016-10-07

Comitter:

Jankoekenpan

Date:

Fri Oct 07 15:39:45 2016 +0000

Commit message:

initial commit. program compiles but doesn't seem to be working.;

Changed in this revision

diff -r 000000000000 -r 35f03aa18046 HIDScope.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/HIDScope.lib	Fri Oct 07 15:39:45 2016 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/tomlankhorst/code/HIDScope/#188304906687

diff -r 000000000000 -r 35f03aa18046 QEI.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/QEI.lib	Fri Oct 07 15:39:45 2016 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/aberk/code/QEI/#5c2ad81551aa

diff -r 000000000000 -r 35f03aa18046 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Oct 07 15:39:45 2016 +0000
@@ -0,0 +1,132 @@
+#include "mbed.h"
+#include "math.h"
+#include "HIDScope.h"
+#include "QEI.h"
+
+// ------ Hardware Interfaces -------
+
+const PinName motor1dir = D7;
+const PinName motor1PWM = D6;
+DigitalOut motor1direction(motor1dir);
+PwmOut motor1control(motor1PWM);
+
+const PinName button1name = D10; //for some reason D8 does not seem to work. wtf!
+const PinName pot1name = A0;
+InterruptIn button1(button1name);
+AnalogIn potMeterIn(pot1name);
+
+const PinName encoderpin1 = D13;
+const PinName encoderpin2 = D12;
+QEI qei(encoderpin1, encoderpin2, NC, 32);
+
+// ------- Objects used -------
+
+// Ticker which controls the mother every 1/100 of a second.
+Ticker controlTicker;
+
+Ticker debugTicker;
+Serial pc(USBTX, USBRX);
+
+Ticker encoderTicker;
+
+HIDScope scope(1);
+
+// ------- Constants
+
+const float motorGain = 8.4f;
+
+const float MAX_VELOCITY = 8.4f; //radians per second
+
+const bool CLOCKWISE = true;
+
+//1 output rod revolution == 131.25 encoder revolution
+const float RATIO = 1.0f/131.25f;
+
+const float tSample = 1.0f/1000.0f;
+
+const double pi = 3.14159265358979323846;
+
+const int VELOCITY_CHANNEL = 0;
+
+
+// ------ variables
+
+volatile bool direction = CLOCKWISE;
+
+volatile double lastEncoderRead = 0; //angle in radians.
+
+// ------ functions
+
+float getReferenceVelocity() {
+    // Returns reference velocity in rad/s. 
+    return MAX_VELOCITY * potMeterIn.read();
+}
+
+void setMotor1(float motorValue) {
+    // Given motorValue<=1, writes the velocity to the pwm control.
+    // MotorValues outside range are truncated to within range.
+    motor1control.write(fabs(motorValue) > 1 ? 1 : fabs(motorValue));
+}
+
+float feedForwardControl(float referenceVelocity) {
+    // very simple linear feed-forward control
+    // returns motorValue
+    return referenceVelocity / motorGain;
+}
+
+void measureAndControl(void) {
+    // This function measures the potmeter position, extracts a
+    // reference velocity from it, and controls the motor with 
+    // a simple FeedForward controller. Call this from a Ticker.
+    float referenceVelocity = getReferenceVelocity();
+    float motorValue = feedForwardControl(referenceVelocity);
+    setMotor1(motorValue);
+}
+
+void onButtonPress() {
+    // reverses the direction
+    motor1direction.write(direction = !direction);
+    pc.printf("direction: %s\r\n\n", direction == CLOCKWISE ? "clockwise" : "counter clockwise");
+}
+
+void onDebugTick() {
+    
+    pc.printf("pot input: %f\r\n", potMeterIn.read());
+    pc.printf("motorValue: %f\r\n", feedForwardControl(getReferenceVelocity()));
+    pc.printf("\n\n\n");
+
+}
+
+void encoderTick() {
+    int pulses = qei.getPulses();
+    
+    //calculate the total angle that is travelel so far.
+    double radians = (pulses / tSample) * ((2*pi)/32);
+    
+    //approximate the derivative for the angular velocity.
+    double xdif = radians - lastEncoderRead;
+    double xderiv = xdif / tSample;
+    
+    //TODO apply a BiQuard filter in between here.
+    
+    //send velocity to HIDScope
+    scope.set(VELOCITY_CHANNEL, xderiv);
+    scope.send();
+    
+    //update lastEncoderRead such that it can be used for next time
+    lastEncoderRead = radians;
+}
+
+int main()
+{
+    pc.baud(115200);
+    qei.reset();
+    
+    encoderTicker.attach(&encoderTick, tSample);
+    controlTicker.attach(&measureAndControl, 1.0f/100.0f);
+    button1.fall(&onButtonPress);
+    
+    debugTicker.attach(&onDebugTick, 1);
+    
+    while (true);
+}
\ No newline at end of file

diff -r 000000000000 -r 35f03aa18046 mbed.bld
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Fri Oct 07 15:39:45 2016 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/aae6fcc7d9bb
\ No newline at end of file