Amber Tang
123
Fork of
JS_1motor_20170707_ok
by 
Amber Tang
Diff: main.cpp
- Revision:
- 0:39ee6f693e49
- Child:
- 1:bfdd210b730a
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Sat Jul 08 09:03:42 2017 +0000
@@ -0,0 +1,430 @@
+/* "PID.h"--- https://developer.mbed.org/users/aberk/code/PID/ */
+/* "Servo.h"-- https://developer.mbed.org/users/andrewrussell/code/Servo/file/4c315bcd91b4/Servo.cpp */
+/* JS using 3.3V */
+
+#include "mbed.h"
+#include "PID.h"
+#include "Servo.h"
+//****************************************************************************
+int encoderClickCount = 0; // hold the signed value corresponding to the number of clicks left or right since last sample
+int previousEncoderState = 0; // keep a record of the last actioned sample state of the Qb+Qa outputs for comparison on each interrupt
+float x, y, press = 0;
+float cx, cy = 0;
+int px, py = 0;
+int bia = 3;
+float X_pos, Y_pos, CW_CCW1, CW_CCW2, stopRun = 0;
+//****************************************************************************
+DigitalOut StopRun(D15);
+DigitalOut Brake(D14);
+DigitalOut AR(D9);
+DigitalOut X1_CW_CCW(PC_6);
+DigitalOut X2_CW_CCW(D2);
+DigitalOut Y1_CW_CCW(PC_8);
+DigitalOut Y2_CW_CCW(PC_9);
+
+Servo X_PWM(D6);
+Servo Y_PWM(D8);
+
+//DigitalIn phaseA(A0); // phase A of the quadrature encoder
+//DigitalIn phaseB(A1); // phase B of the quadrature encoder
+DigitalIn phaseA( PC_2 ); // phase a of the quadrature encoder
+DigitalIn phaseB( PC_3 ); // phase b of the quadrature encoder
+AnalogIn vx(A2);
+AnalogIn vy(A3);
+AnalogIn ps(A4);
+
+
+void quadratureDecoder( void )
+{
+ int currentEncoderState = (phaseB.read() << 1) + phaseA.read(); // create a two bit value out of phaseB and phaseA
+
+ if( currentEncoderState == previousEncoderState )
+ {
+ return;
+ }
+
+ switch( previousEncoderState )
+ {
+ case 0:
+ if( currentEncoderState == 2 )
+ {
+ encoderClickCount--;
+
+ }
+ else if( currentEncoderState == 1 )
+ {
+ encoderClickCount++;
+
+ }
+ break;
+
+ case 1:
+ if( currentEncoderState == 0 )
+ {
+ encoderClickCount--;
+
+ }
+ else if( currentEncoderState == 3 )
+ {
+ encoderClickCount++;
+
+ }
+ break;
+
+ case 2:
+ if( currentEncoderState == 3 )
+ {
+ encoderClickCount--;
+
+ }
+ else if( currentEncoderState == 0 )
+ {
+ encoderClickCount++;
+
+ }
+ break;
+
+ case 3:
+ if( currentEncoderState == 1 )
+ {
+ encoderClickCount--;
+
+ }
+ else if( currentEncoderState == 2 )
+ {
+ encoderClickCount++;
+
+ }
+ break;
+
+ default:
+ break;
+ }
+ previousEncoderState = currentEncoderState;
+}
+
+int getClicks( void )
+{
+ int res = encoderClickCount; // this allows the knob to be rotated "while im not looking at it' and still return the
+
+ // encoderClickCount = 0; // actual number of clicks that have been rotated since last time I was checking it.
+
+ return res;
+}
+
+int main()
+{
+
+ Serial pc( USBTX, USBRX );
+ pc.baud(115200);
+ Ticker sampleTicker; // create a timer to sample the encoder
+ int currentClicks;
+
+ phaseA.mode( PullUp ); // phaseA is set with a built in pull-up resistor
+ phaseB.mode( PullUp ); // phaseB is set with a built in pull-up resistor
+ sampleTicker.attach_us( &quadratureDecoder, 100 ); // make the quadrature decoder function check the knob once every 1000us = 1ms
+
+
+ //PWM.calibrate(0.02, 0.001, 0.002); //50HZ
+ //PWM.calibrate(0.02, 0.02*0.02, 1*0.02); //50HZ
+ //PWM.calibrate(0.0001, 0.08*0.0001, 1*0.0001); //10kHZ
+ //PWM.calibrate(0.00001, 0.08*0.00001, 1*0.00001); //100kHZ
+ //PWM.calibrate(0.00001, 0.2*0.00001, 0.8*0.00001); //100kHZ
+ X_PWM.calibrate(0.00005, 0.08*0.00005, 1*0.00005); //20kHZ
+ Y_PWM.calibrate(0.00005, 0.08*0.00005, 1*0.00005); //20kHZ
+ //PWM.calibrate(0.000025, 0*0.000025, 1*0.000025); //40kHZ
+ //PWM.calibrate(0.000005, 0*0.000025, 1*0.000025); //200kHZ
+ cx = vx.read();
+ cy = vy.read();
+ AR.write(1);
+
+ pc.printf( "Encoder test started\r\n" );
+ while( 1 )
+ {
+ currentClicks = getClicks();
+ Brake.write(1);
+ x = vx.read();
+ y = vy.read();
+ /* x-axis re-arrange */
+ if(x > cx)
+ {
+ px = (x-cx)/(1-cx)*100;
+ if(px < bia)
+ {
+ px = 0;
+ stopRun = 1;
+ CW_CCW1 = 1;
+ CW_CCW2 = 0;
+ X_pos = 0;
+ }
+ else
+ {
+ stopRun = 0;
+ CW_CCW1 = 1;
+ CW_CCW2 = 0;
+ X_pos = 0.5;
+ }
+ }
+ else if(x < cx)
+ {
+ px = (x-cx)/(cx-0)*100;
+ if(px > -bia)
+ {
+ px = 0;
+ stopRun = 1;
+ CW_CCW1 = 0;
+ CW_CCW2 = 1;
+ X_pos = 0;
+ }
+ else
+ {
+ stopRun = 0;
+ CW_CCW1 = 0;
+ CW_CCW2 = 1;
+ X_pos = 0.5;
+ }
+ }
+ else
+ px=cx;
+ /* y-axis re-arrange */
+ if(y > cy)
+ {
+ py = (y-cy)/(1-cy)*100;
+ if(py < bia)
+ {
+ py = 0;
+ stopRun = 1;
+ CW_CCW1 = 1;
+ CW_CCW2 = 0;
+ Y_pos = 0;
+ }
+ else
+ {
+
+ stopRun = 0;
+ CW_CCW1 = 1;
+ CW_CCW2 = 0;
+ Y_pos = 0.5;
+ }
+ }
+ else if(y < cy)
+ {
+ py = (y-cy)/(cy-0)*100;
+ if(py > -bia)
+ {
+ py = 0;
+ stopRun = 1;
+ CW_CCW1 = 0;
+ CW_CCW2 = 1;
+ Y_pos = 0;
+ }
+ else
+ {
+ stopRun = 0;
+ CW_CCW1 = 0;
+ CW_CCW2 = 1;
+ Y_pos = 0.5;
+ }
+ }
+ else
+ {
+ py=cy;
+ }
+ // while( 1 )
+// {
+// currentClicks = getClicks();
+// Brake.write(1);
+// x = vx.read();
+// y = vy.read();
+// /* x-axis re-arrange */
+// if(x > cx)
+// {
+// px = (x-cx)/(1-cx)*100;
+// if(px < bia)
+// {
+// px = 0;
+// StopRun.write(1);
+// X1_CW_CCW.write(1); // clockwise:0 counterclockwise:1
+// X2_CW_CCW.write(0); // clockwise:0 counterclockwise:1
+// Y1_CW_CCW.write(1); // clockwise:0 counterclockwise:1
+// Y2_CW_CCW.write(0); // clockwise:0 counterclockwise:1
+// X_pos = 0;
+// Y_pos = 0;
+// }
+// else
+// {
+// StopRun.write(0);
+//// CW_CCW.write(0); // clockwise:0 counterclockwise:1
+// X1_CW_CCW.write(1); // clockwise:0 counterclockwise:1
+// X2_CW_CCW.write(0); // clockwise:0 counterclockwise:1
+// Y1_CW_CCW.write(1); // clockwise:0 counterclockwise:1
+// Y2_CW_CCW.write(0); // clockwise:0 counterclockwise:1
+// X_pos = 0.5;
+// Y_pos = 0;
+// }
+// }
+// else if(x < cx)
+// {
+// px = (x-cx)/(cx-0)*100;
+// if(px > -bia)
+// {
+// px = 0;
+// StopRun.write(1);
+//// CW_CCW.write(1); // clockwise:0 counterclockwise:1
+// X1_CW_CCW.write(0); // clockwise:0 counterclockwise:1
+// X2_CW_CCW.write(1); // clockwise:0 counterclockwise:1
+// Y1_CW_CCW.write(0); // clockwise:0 counterclockwise:1
+// Y2_CW_CCW.write(1); // clockwise:0 counterclockwise:1
+// X_pos = 0;
+// Y_pos = 0;
+// }
+// else
+// {
+// StopRun.write(0);
+//// CW_CCW.write(1); // clockwise:0 counterclockwise:1
+// X1_CW_CCW.write(0); // clockwise:0 counterclockwise:1
+// X2_CW_CCW.write(1); // clockwise:0 counterclockwise:1
+// Y1_CW_CCW.write(0); // clockwise:0 counterclockwise:1
+// Y2_CW_CCW.write(1); // clockwise:0 counterclockwise:1
+// X_pos = 0.5;
+// Y_pos = 0;
+// }
+// }
+// else
+// px=cx;
+// /* y-axis re-arrange */
+// if(y > cy)
+// {
+// py = (y-cy)/(1-cy)*100;
+// if(py < bia)
+// {
+// py = 0;
+// StopRun.write(1);
+// X1_CW_CCW.write(1); // clockwise:0 counterclockwise:1
+// X2_CW_CCW.write(0); // clockwise:0 counterclockwise:1
+// Y1_CW_CCW.write(1); // clockwise:0 counterclockwise:1
+// Y2_CW_CCW.write(0); // clockwise:0 counterclockwise:1
+// X_pos = 0;
+// Y_pos = 0;
+// }
+// else
+// {
+//
+// StopRun.write(0);
+// X1_CW_CCW.write(1); // clockwise:0 counterclockwise:1
+// X2_CW_CCW.write(0); // clockwise:0 counterclockwise:1
+// Y1_CW_CCW.write(1); // clockwise:0 counterclockwise:1
+// Y2_CW_CCW.write(0); // clockwise:0 counterclockwise:1
+// X_pos = 0;
+// Y_pos = 0.5;
+// }
+// }
+// else if(y < cy)
+// {
+// py = (y-cy)/(cy-0)*100;
+// if(py > -bia)
+// {
+// py = 0;
+// StopRun.write(1);
+// X1_CW_CCW.write(0); // clockwise:0 counterclockwise:1
+// X2_CW_CCW.write(1); // clockwise:0 counterclockwise:1
+// Y1_CW_CCW.write(0); // clockwise:0 counterclockwise:1
+// Y2_CW_CCW.write(1); // clockwise:0 counterclockwise:1
+// X_pos = 0;
+// Y_pos = 0;
+// }
+// else
+// {
+// StopRun.write(0);
+// X1_CW_CCW.write(0); // clockwise:0 counterclockwise:1
+// X2_CW_CCW.write(1); // clockwise:0 counterclockwise:1
+// Y1_CW_CCW.write(0); // clockwise:0 counterclockwise:1
+// Y2_CW_CCW.write(1); // clockwise:0 counterclockwise:1
+// X_pos = 0;
+// Y_pos = 0.5;
+// }
+// }
+// else
+// {
+// py=cy;
+// }
+ /*if(x > cx)
+ {
+ px = (x-cx)/(1-cx)*100;
+ if(px < bia)
+ {
+ px = 0;
+ StopRun.write(1);
+ CW_CCW.write(0); // clockwise:0 counterclockwise:1
+ pos = 0;
+ }
+ else
+ {
+ StopRun.write(0);
+ CW_CCW.write(0); // clockwise:0 counterclockwise:1
+ pos=px*0.01;
+ }
+ }
+ else if(x < cx)
+ {
+ px = (x-cx)/(cx-0)*100;
+ if(px > -bia)
+ {
+ px = 0;
+ StopRun.write(1);
+ CW_CCW.write(1); // clockwise:0 counterclockwise:1
+ pos = 0;
+ }
+ else
+ {
+ StopRun.write(0);
+ CW_CCW.write(1); // clockwise:0 counterclockwise:1
+ pos=px*(-0.01);
+ }
+ }
+ else
+ px=cx;
+ /* y-axis re-arrange */
+ /*if(y > cy)
+ {
+ py = (y-cy)/(1-cy)*100;
+ if(py < bia)
+ py = 0;
+ }
+ else if(y < cy)
+ {
+ py = (y-cy)/(cy-0)*100;
+ if(py > -bia)
+ py = 0;
+ }
+ else
+ {
+ py=cy;
+ }
+ */
+ StopRun.write(stopRun);
+ X1_CW_CCW.write(CW_CCW1); // clockwise:0 counterclockwise:1
+ X2_CW_CCW.write(CW_CCW2); // clockwise:0 counterclockwise:1
+ Y1_CW_CCW.write(CW_CCW1); // clockwise:0 counterclockwise:1
+ Y2_CW_CCW.write(CW_CCW2); // clockwise:0 counterclockwise:1
+ X_PWM.write(X_pos);
+ Y_PWM.write(Y_pos);
+
+
+ press = ps.read();
+ if(press < 0.008)
+ printf("Pressed: %d, %d\n",px,py);
+ else
+ printf(" : %d, %d\n",px,py);
+
+ wait_ms(0.1);
+
+ if( currentClicks != 0 )
+ {
+ //pc.printf( "%f, %d, %f || click count = %d\r\n", x, px, pos, currentClicks/*, previousEncoderState */);
+ //pc.printf("%f, %d, %f\n",x,px,pos);
+ pc.printf( "%f || %f || click count = %d\r\n", X_pos, Y_pos, currentClicks/*, previousEncoderState */);
+ }
+ wait_ms( 20 );
+ }
+}
\ No newline at end of file