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