ManualControl
Dependencies: TPixy-Interface
Fork of MbedOS_Robot by
Revision 19:4fbffc755ed7, committed 2018-03-23
- Comitter:
- asobhy
- Date:
- Fri Mar 23 19:03:35 2018 +0000
- Parent:
- 18:db6d9fc1ebd0
- Child:
- 20:9118203f7c9c
- Commit message:
- increasing sampling rate manual control
Changed in this revision
--- a/PiControlThread.cpp Tue Mar 13 22:14:24 2018 +0000 +++ b/PiControlThread.cpp Fri Mar 23 19:03:35 2018 +0000 @@ -86,7 +86,7 @@ // Specify address of the PeriodicInt ISR as PiControllerISR, specify the interval // in seconds between interrupts, and start interrupt generation: - PeriodicInt.attach(&PeriodicInterruptISR, 0.05); // 50ms sampling rate + PeriodicInt.attach(&PeriodicInterruptISR, 0.0125); // 12.5ms sampling period -> 80kHz } @@ -107,8 +107,8 @@ DE0_read(&sensors); // might not be needed - sensors.dp_right = SaturateValue(sensors.dp_right, 560); - sensors.dp_left = SaturateValue(sensors.dp_left, 560); + sensors.dp_right = SaturateValue(sensors.dp_right, 140); + sensors.dp_left = SaturateValue(sensors.dp_left, 140); // Maximum velocity at dPostition = 560 is vel = 703 velR = (float)((6135.92 * sensors.dp_right) / sensors.dt_right) ; @@ -116,34 +116,6 @@ // Maximum velocity at dPostition = 560 is vel = 703 velL = (float)((6135.92 * sensors.dp_left) / sensors.dt_left) ; - /*********************Differential Start*******************************/ - // Inputs are Speed Setpoint and Steering Setpoint - // The Inputs for the Steering are specified in the CameraThread - // and set at the center. - // The distance between the object and the image should be set to 1 meter - // If distance decrease then speed should stop. - // If distance increase then speed should increase. - - // If object is moving away from the the robot increase robot speed - /*cameraData_mutex.lock(); - if(DistanceError > 0) - { - // Proportional controller - Setpoint = (Kd*DistanceError); - Setpoint = SaturateValue(Setpoint,560); - } - // If object is at the set distance limit or less then do not move. - else if(DistanceError <= 0) - { - Setpoint = 0; - } - - // Decoupled drive - setpointR = Setpoint + (Ks*SteeringError); - setpointL = Setpoint - (Ks*SteeringError); - cameraData_mutex.unlock(); - */ - /********************Manual Setpoint and Steering**********************/ mutexSetpoint.lock(); setpointR = Setpoint + (Ks*SteeringError); @@ -151,8 +123,8 @@ mutexSetpoint.unlock(); // Make sure our limit is not exceeded - setpointR = SaturateValue(setpointR, 560); - setpointL = SaturateValue(setpointL, 560); + setpointR = SaturateValue(setpointR, 140); + setpointL = SaturateValue(setpointL, 140); /*********************Differential End*********************************/
--- a/main.cpp Tue Mar 13 22:14:24 2018 +0000 +++ b/main.cpp Fri Mar 23 19:03:35 2018 +0000 @@ -45,6 +45,6 @@ } consoleUI(); - Thread::wait(100); // Go to sleep for 500 ms + Thread::wait(20); // Go to sleep for 20 ms } }
--- a/ui.cpp Tue Mar 13 22:14:24 2018 +0000 +++ b/ui.cpp Fri Mar 23 19:03:35 2018 +0000 @@ -186,12 +186,10 @@ // by incrementing u else if(x == 'w') { setpointR_mutex.lock(); - if ( setpointR < 560 ) - { - //setpoint = setpoint + SPEED_STEP; - setpointR = 280; - } + //setpoint = setpoint + SPEED_STEP; + setpointR = 25; setpointR_mutex.unlock(); + bluetooth.printf("\r\n %5d", setpointR); } @@ -200,43 +198,39 @@ else if(x == 's') { setpointR_mutex.lock(); - if (setpointR > -560) - { - setpointR = -280; - //setpoint = setpoint - SPEED_STEP; - } - + + setpointR = -25; + //setpoint = setpoint - SPEED_STEP; + setpointR_mutex.unlock(); // display speed - bluetooth.printf("\r\n %5d", setpointR); + //bluetooth.printf("\r\n %5d", setpointR); } /******************************LEFT MOTOR**************************************/ else if (x=='i') { setpointL_mutex.lock(); - if ( setpointL < 560 ) - { + + //setpoint = setpoint + SPEED_STEP; - setpointL = 280; - } + setpointL = 25; + setpointL_mutex.unlock(); - bluetooth.printf("\r\n %5d", setpointL); + //bluetooth.printf("\r\n %5d", setpointL); } else if (x=='k') { setpointL_mutex.lock(); - if (setpointL > -560) - { - setpointL = -280; - //setpoint = setpoint - SPEED_STEP; - } - + + setpointL = -25; + //setpoint = setpoint - SPEED_STEP; + setpointL_mutex.unlock(); // display speed - bluetooth.printf("\r\n %5d", setpointL); + //bluetooth.printf("\r\n %5d", setpointL); } /******************************END MOTOR SETPOINT******************************/ @@ -286,7 +280,7 @@ // by incrementing u else if(x == 'w') { mutexSetpoint.lock(); - Setpoint = 100; + Setpoint = -25; mutexSetpoint.unlock(); } @@ -295,7 +289,7 @@ // by decrementing u else if(x == 's') { mutexSetpoint.lock(); - Setpoint = -100; + Setpoint = 25; mutexSetpoint.unlock(); } @@ -304,13 +298,13 @@ else if (x=='a') { mutexSetpoint.lock(); - SteeringError = 300; + SteeringError = 75; mutexSetpoint.unlock(); } else if (x=='d') { mutexSetpoint.lock(); - SteeringError = -300; + SteeringError = -75; mutexSetpoint.unlock(); } // error wrong input @@ -325,4 +319,6 @@ } + //bluetooth.printf("\r\nPos: %d, dP: %d, dT: %d, Kp: %f, Ki: %f, vel: %d, e: %d", position, dPosition, dTime, Kp, Ki, vel, e); + }