![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
123
Fork of JS_1motor_20170707_ok by
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