Amber Tang
123
Fork of
JS_1motor_20170707_ok
by 
Amber Tang
main.cpp
- Committer:
- Amber77
- Date:
- 2017-07-08
- Revision:
- 1:bfdd210b730a
- Parent:
- 0:39ee6f693e49
- Child:
- 2:868e7c902ba6
File content as of revision 1:bfdd210b730a:
/* "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(D7);
//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 );
}
}