Milo Pan
2d drive position tracking
Dependencies: MotorV2 QEI mbed
Revision 0:d4bb648b7f2c, committed 2016-11-14
- Comitter:
- simplyellow
- Date:
- Mon Nov 14 21:46:17 2016 +0000
- Commit message:
- code with old version commented out
Changed in this revision
diff -r 000000000000 -r d4bb648b7f2c MotorV2.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/MotorV2.lib Mon Nov 14 21:46:17 2016 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/teams/Terrabots/code/MotorV2/#373f15001531
diff -r 000000000000 -r d4bb648b7f2c QEI.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/QEI.lib Mon Nov 14 21:46:17 2016 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/aberk/code/QEI/#5c2ad81551aa
diff -r 000000000000 -r d4bb648b7f2c main.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Mon Nov 14 21:46:17 2016 +0000
@@ -0,0 +1,210 @@
+#include "QEI.h"
+#include "mbed.h"
+#include "Motor.h"
+
+/*
+ Written by Milo Pan with the help of Jacob Callahan
+*/
+
+/*
+Use X4 encoding.
+QEI wheel(p29, p30, NC, 624, QEI::X4_ENCODING);
+QEI constructor(CHANNEL A, CHANNEL B,PinName index (NC), int pulsesPerRev, Encoding encoding=X2_ENCODING)
+Use X2 encoding by default.
+Encoding x4 = x4_ENCODING;
+*/
+
+/*
+Measurement Instructions
+in order to convert the numerical value (outputted by this program) to Inches traveled,
+measurements need to be taken (i.e. have the truck travel 40" for 10 trials, average the values, and find the factor
+*/
+
+Serial pc(USBTX, USBRX);
+
+QEI wheel (p29, p30, NC, 128, QEI::X4_ENCODING); //orange, yellow wires, VU 5 VOLTS, BOTTOM 2x2 pins on WW02
+float pulseCount; //number of pulses detected by encoder
+float encoding = 2; //X2
+float circumference = 13.25; //inches of circumference
+float distance;
+
+AnalogIn pot(p15);
+float value;
+float angle;
+float zero;
+
+float PI = 3.14159;
+
+/* VERSION 1
+float x = 0;
+float dx;
+float y = 0;
+float dy;
+float dispAngle; // theta d
+float r; // r
+float c; // c, chord length
+
+float segment; // intermediary variable for ease of calculation
+
+float heading; // heading of front
+*/
+
+
+// VERSION 2
+float df;
+float db;
+float circumference2;
+float proportion;
+float scaledDown;
+float W;
+float R;
+int dir; //1 CCW, 2 CW, 0 straight
+float heading = 0.0;
+float dispAngleRad;
+float dispAngleDeg;
+float x;
+float dx;
+float y;
+float dy;
+
+float totalDistance;
+int i;
+
+
+
+void measure() {
+ // raw measurements of pot, encoder
+ //(pulse count / X * N) * (1 / PPI)
+ pulseCount = (float) wheel.getPulses();
+ /* conversion to relevant values: distance of arc traveled, angle of turn
+ | suppose the truck is a two-segment body
+ / <- angle between bottom leg and the vertical = angle of turn
+ the 20 and 68 were arbitrary and determined from testing, respectively
+ the distance is in inches
+ */
+ distance = pulseCount*circumference/encoding/20/68;
+ // with half inch correction after testing extensively
+ /*if (distance > 0) {
+ distance-=0.5;
+ }
+ */
+}
+
+/* VERSION 1
+void calculateDisplacement() {
+ /
+ Premise:
+ Suppose the arc s (distance), the distance traveled by the
+ turning truck, is defined by two connected points on
+ a circle with radius r.
+ /
+
+ // convert angle to radians
+ angle = angle*PI/180;
+ // r = s/angle, for distance of arc s
+ r = distance/(angle);
+ // c = 2*r*sin(angle/2) for chord c
+ c = 2*r*sin(angle/2);
+ // intermediate value
+ segment = (r*(1-cos(angle)));
+ // calculation of angle of displacement
+ dispAngle = acos(segment/c);
+ // calculation of displacement
+ dx = c*cos(dispAngle+heading);
+ dy = c*sin(dispAngle+heading);
+ x = x - r + dx;
+ y = y + dy;
+ wait(1.0);
+}
+*/
+
+void calculateDisplacement() {
+ df = 4.5; // distance from pivot to front wheel
+ db = 4.25; // distance from pivot to back wheel
+ // W = 2*R*sin(0.5*angle);
+ // W = shortest distance between wheels
+ angle = angle*PI/180;
+ W = sqrt(df*df+db*db-2*df*db*cos(PI-angle));
+ R = W/(2*sin(0.5*angle));
+
+ circumference2 = 2*PI*R; // inches
+ proportion = distance/circumference2;
+ scaledDown = proportion - floor(proportion);
+ dispAngleRad = scaledDown*2*PI;
+ dispAngleDeg = scaledDown*360;
+ dx = R*cos(dispAngleRad + heading);
+ dy = R*sin(dispAngleRad + heading);
+
+ if(dir == 1) { // CCW
+ x = 0 - R + dx;
+ y = dy;
+ } else if (dir == 2) { // CW
+ y = dy;
+ if((dispAngleDeg < 90) || (dispAngleDeg > 270)) {
+ x = dx;
+ } else {
+ x = R - dx;
+ }
+ } else {
+ x = 0;
+ y = distance;
+ }
+ heading += dispAngleRad;
+ wait(1.0);
+}
+/*
+dir
+and straight accuracy
+*/
+
+int main() {
+ // required to set truck straight before start up to calibrate to zero
+ zero = (float) pot;
+ while(i<5) {
+ printf("%d set angle \n\r",i);
+ wait(1);
+ i++;
+ }
+ if (totalDistance == 0) { // pot turns constant after the distance turns non zero
+ value = (float) pot;
+ // appropriate conversion from potentiometer float value to angle
+ angle = (value-zero)*300;
+ printf("%f",angle);
+ }
+ distance = 0;
+ totalDistance = 0;
+ heading = 0;
+ while(1){
+ measure();
+
+ calculateDisplacement();
+
+ totalDistance+=distance;
+ if(totalDistance>0) {//@@@@@@@@@@check dir with angle vals@@@@@@@
+ if (angle > 0.5) {
+ dir = 1;//ccw
+ } else if (angle < -0.5) {
+ dir = 2;//cw
+ } else {
+ dir = 0;
+ }
+ }
+ distance = 0;
+
+ /*
+ Notes:
+ At startup:
+ distance = 0
+ angle =! 0
+ */
+
+ // for debugging
+ pc.printf("Distance %f, Angle %f\n\r", totalDistance, angle*180/PI);
+ pc.printf("(X,Y): %f %f\n\r", x, y);
+ pc.printf("(DX,DY): %f %f\n\r", dx, dy);
+ pc.printf("heading: %f\n\r", heading);
+ pc.printf("----------------------------\n\r");
+ }
+}
+
+
diff -r 000000000000 -r d4bb648b7f2c mbed.bld --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Mon Nov 14 21:46:17 2016 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/25aea2a3f4e3 \ No newline at end of file