ManualControl
Dependencies: TPixy-Interface
Fork of MbedOS_Robot by
Revision 5:b29220d29022, committed 2018-02-09
- Comitter:
- asobhy
- Date:
- Fri Feb 09 20:26:04 2018 +0000
- Parent:
- 4:417e475239c7
- Commit message:
- time constant calculation
Changed in this revision
PiControlThread.cpp | Show annotated file Show diff for this revision Revisions of this file |
ui.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/PiControlThread.cpp Fri Feb 09 18:37:11 2018 +0000 +++ b/PiControlThread.cpp Fri Feb 09 20:26:04 2018 +0000 @@ -6,11 +6,16 @@ #include "PiController.h" extern int setpoint; +extern bool start_counting; +extern Serial bluetooth; uint16_t ID, dTime; int dPosition; int vel; int32_t U; +int counter =0 ; + +int dPosition_values[1000]; void PiControlThread(void const *); void PeriodicInterruptISR(void); @@ -28,7 +33,7 @@ /******************************************************************************/ // Declare PeriodicInterruptThread as a thread/process -osThreadDef(PiControlThread, osPriorityRealtime, 1024); +osThreadDef(PiControlThread, osPriorityRealtime, 1024); Ticker PeriodicInt; // Declare a timer interrupt: PeriodicInt @@ -40,13 +45,13 @@ DE0_init(); // initialize FPGA motorDriver_init(); // initialize motorDriver PiController_init(2.2,0.01); // initialize the PI Controller gains and initialize variables - + PiControlId = osThreadCreate(osThread(PiControlThread), NULL); // 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.001); // 50ms sampling rate + } @@ -55,41 +60,69 @@ *******************************************************************************/ void PiControlThread(void const *argument) { - - while (true) - { + + // time constant calc + setpoint = 140; + + while (true) { osSignalWait(0x01, osWaitForever); // Go to sleep until signal, SignalPi, is received. - + // get incremental position and time from QEI DE0_read(&ID, &dPosition, &dTime); SaturateValue(dPosition, 560); - + // maximum velocity at dPostition = 560 is vel = 703 vel = (float)((6135.92 * dPosition) / dTime) ; - - U = PiController(setpoint,dPosition); - - if (U >= 0) - { + + //U = PiController(setpoint,dPosition); + + // time constnant calc. + U = setpoint/28; + + if (U >= 0) { motorDriver_forward(U); + } else if (U < 0) { + motorDriver_reverse(U); } - else if (U < 0) - { - motorDriver_reverse(U); - } - + + + // start our calculations + if( start_counting == true ) { + float time_constant = 0; + dPosition_values[counter] = dPosition; + counter++; + + // if we reach our setpoint + if (dPosition > 8 ) { + // stop counting time + start_counting = false; + // find the value where dPosition = 316 + for(int i=0; i<1000; i++) { + if( dPosition_values[i] >= 7 ) { + time_constant = counter; + //time_constant = time_constant; + bluetooth.printf("\ntime constant: %f", time_constant); + } + } + } + } + + + + + } - + } /******************************************************************************* -* the interrupt below occures every 250ms as setup in the main function during +* the interrupt below occures every 250ms as setup in the main function during * initialization * ******** Period Timer Interrupt Handler ******** *******************************************************************************/ void PeriodicInterruptISR(void) { // Send signal to the thread with ID, PeriodicInterruptId, i.e., PeriodicInterruptThread. - osSignalSet(PiControlId,0x1); + osSignalSet(PiControlId,0x1); }
--- a/ui.cpp Fri Feb 09 18:37:11 2018 +0000 +++ b/ui.cpp Fri Feb 09 20:26:04 2018 +0000 @@ -23,6 +23,7 @@ int16_t position; +bool start_counting = false; /****************************************************************************** @@ -140,7 +141,8 @@ if ( setpoint < 506 ) { //setpoint = setpoint + SPEED_STEP; - setpoint = 400; + setpoint = 420; + start_counting = true; } setpoint_mutex.unlock(); @@ -191,8 +193,12 @@ //bluetooth.printf("\r\nPos: %d, dP: %d, dT: %d, Kp: %f, Ki: %f, vel: %d, e: %d", position, dPosition, dTime, Kp, Ki, vel, e); - bluetooth.printf("\r\ndP: %d, vel: %d, Kp: %f, Ki: %f, e: %d, xState: %d", dPosition, vel, Kp, Ki, e, xState); + //bluetooth.printf("\r\ndP: %d, vel: %d, Kp: %f, Ki: %f, e: %d, xState: %d", dPosition, vel, Kp, Ki, e, xState); //bluetooth.printf("\r\ne: %d, Pos: %d, dP: %d, xState: %d, u: %d, dT: %d", e, position, dPosition, xState, u, dTime); + bluetooth.printf("\r\n%d", dPosition); + + + }