The subsystem design/basis for the final project
Dependencies: mbed-rtos mbed-src pixylib
robot.cpp@16:73db7ef2deb6, 2016-03-28 (annotated)
- Committer:
- balsamfir
- Date:
- Mon Mar 28 21:39:06 2016 +0000
- Revision:
- 16:73db7ef2deb6
- Parent:
- 15:caa5a93a31d7
- Child:
- 17:47e107f9587b
First working ps3 manual control
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
balsamfir | 2:2bc519e14bae | 1 | #include "robot.h" |
balsamfir | 2:2bc519e14bae | 2 | #include "global.h" |
balsamfir | 3:dfb6733ae397 | 3 | |
balsamfir | 3:dfb6733ae397 | 4 | // Global Definitions |
balsamfir | 3:dfb6733ae397 | 5 | // ---------------------------------------------------------------- |
balsamfir | 2:2bc519e14bae | 6 | |
balsamfir | 6:52686c25e4af | 7 | // Function prototypes |
balsamfir | 6:52686c25e4af | 8 | // ---------------------------------------------------------------- |
balsamfir | 6:52686c25e4af | 9 | void MotorISR(void); |
balsamfir | 6:52686c25e4af | 10 | void NavigationISR(void); |
balsamfir | 6:52686c25e4af | 11 | void CollisionISR(void); |
balsamfir | 6:52686c25e4af | 12 | void WatchdogISR(void const *n); |
balsamfir | 6:52686c25e4af | 13 | void MotorThread(void const *argument); |
balsamfir | 6:52686c25e4af | 14 | void NavigationThread(void const *argument); |
balsamfir | 14:2d609d465f00 | 15 | void UpdateGains(char key, float *increment); |
balsamfir | 14:2d609d465f00 | 16 | void ShutDown(void); |
balsamfir | 6:52686c25e4af | 17 | |
balsamfir | 3:dfb6733ae397 | 18 | // Interrupt and thread control |
balsamfir | 15:caa5a93a31d7 | 19 | osTimerId oneShotId; |
balsamfir | 3:dfb6733ae397 | 20 | osThreadId motorId, navigationId, wdtId; |
balsamfir | 3:dfb6733ae397 | 21 | osThreadDef(MotorThread, osPriorityRealtime, DEFAULT_STACK_SIZE); |
balsamfir | 3:dfb6733ae397 | 22 | osThreadDef(NavigationThread, osPriorityAboveNormal, DEFAULT_STACK_SIZE); |
balsamfir | 3:dfb6733ae397 | 23 | osTimerDef(Wdtimer, WatchdogISR); |
balsamfir | 3:dfb6733ae397 | 24 | int32_t motorSignal, navigationSignal; |
balsamfir | 3:dfb6733ae397 | 25 | Ticker motorPeriodicInt; |
balsamfir | 3:dfb6733ae397 | 26 | Ticker sensorPeriodicInt; |
balsamfir | 2:2bc519e14bae | 27 | |
balsamfir | 3:dfb6733ae397 | 28 | // Mutex to protect left and right motor setpoints |
balsamfir | 3:dfb6733ae397 | 29 | osMutexId motorMutex; |
balsamfir | 3:dfb6733ae397 | 30 | osMutexDef(motorMutex); |
balsamfir | 2:2bc519e14bae | 31 | |
balsamfir | 3:dfb6733ae397 | 32 | // Set points and display variables |
balsamfir | 3:dfb6733ae397 | 33 | float leftMotor, rightMotor; |
balsamfir | 6:52686c25e4af | 34 | int x, height; |
balsamfir | 3:dfb6733ae397 | 35 | float speed, steering; |
balsamfir | 2:2bc519e14bae | 36 | |
balsamfir | 3:dfb6733ae397 | 37 | // Functions |
balsamfir | 3:dfb6733ae397 | 38 | // ---------------------------------------------------------------- |
balsamfir | 15:caa5a93a31d7 | 39 | void InitRobot(void) { |
balsamfir | 3:dfb6733ae397 | 40 | leftPwm.period(PWM_PERIOD); |
balsamfir | 3:dfb6733ae397 | 41 | leftPwm.pulsewidth(0); |
balsamfir | 3:dfb6733ae397 | 42 | rightPwm.period(PWM_PERIOD); |
balsamfir | 3:dfb6733ae397 | 43 | rightPwm.pulsewidth(0); |
balsamfir | 3:dfb6733ae397 | 44 | |
balsamfir | 3:dfb6733ae397 | 45 | motorMutex = osMutexCreate(osMutex(motorMutex)); |
balsamfir | 3:dfb6733ae397 | 46 | |
balsamfir | 3:dfb6733ae397 | 47 | bumper.rise(&CollisionISR); // Attach interrupt handler to rising edge of Bumper |
balsamfir | 2:2bc519e14bae | 48 | |
balsamfir | 3:dfb6733ae397 | 49 | // Start execution of the threads MotorThread, and NavigationThread: |
balsamfir | 3:dfb6733ae397 | 50 | navigationId = osThreadCreate(osThread(NavigationThread), NULL); |
balsamfir | 3:dfb6733ae397 | 51 | motorId = osThreadCreate(osThread(MotorThread), NULL); |
balsamfir | 3:dfb6733ae397 | 52 | |
balsamfir | 2:2bc519e14bae | 53 | // Start the watch dog timer and enable the watch dog interrupt |
balsamfir | 15:caa5a93a31d7 | 54 | oneShotId = osTimerCreate(osTimer(Wdtimer), osTimerOnce, (void *)0); |
balsamfir | 3:dfb6733ae397 | 55 | led3 = 0; // Clear watch dog led3 status |
balsamfir | 2:2bc519e14bae | 56 | |
balsamfir | 3:dfb6733ae397 | 57 | //SPI Initialization |
balsamfir | 3:dfb6733ae397 | 58 | pc.printf("\n\rStarting SPI..."); |
balsamfir | 3:dfb6733ae397 | 59 | deSpi.format(16,1); // 16 bit mode 1 |
balsamfir | 3:dfb6733ae397 | 60 | ioReset = 0; ioReset = 1; |
balsamfir | 3:dfb6733ae397 | 61 | wait_us(10); ioReset = 0; spiReset = 0; spiReset = 1; |
balsamfir | 3:dfb6733ae397 | 62 | wait_us(10); spiReset = 0; |
balsamfir | 14:2d609d465f00 | 63 | pc.printf("\n\rDevice Id: %d \r\n", deSpi.write(0x8004)); // Read count & time for both motors |
balsamfir | 15:caa5a93a31d7 | 64 | } |
balsamfir | 15:caa5a93a31d7 | 65 | |
balsamfir | 15:caa5a93a31d7 | 66 | void AutoTrack(void) { |
balsamfir | 15:caa5a93a31d7 | 67 | char key; |
balsamfir | 6:52686c25e4af | 68 | motorPeriodicInt.attach(&MotorISR, MOTOR_PERIOD); |
balsamfir | 6:52686c25e4af | 69 | sensorPeriodicInt.attach(&NavigationISR, NAVIGATION_PERIOD); |
balsamfir | 2:2bc519e14bae | 70 | |
balsamfir | 2:2bc519e14bae | 71 | while (1) { |
balsamfir | 3:dfb6733ae397 | 72 | osTimerStart(oneShotId, 2000); // Start or restart the watchdog timer interrupt and set to 2000ms. |
balsamfir | 15:caa5a93a31d7 | 73 | if (pc.readable()) { |
balsamfir | 15:caa5a93a31d7 | 74 | key = pc.getc(); |
balsamfir | 15:caa5a93a31d7 | 75 | if (key == 'q') { |
balsamfir | 15:caa5a93a31d7 | 76 | ShutDown(); |
balsamfir | 15:caa5a93a31d7 | 77 | return; |
balsamfir | 14:2d609d465f00 | 78 | } |
balsamfir | 14:2d609d465f00 | 79 | } |
balsamfir | 15:caa5a93a31d7 | 80 | pc.printf("X Coordinate: %d, Height: %d \r\n", x, height); |
balsamfir | 15:caa5a93a31d7 | 81 | pc.printf("Speed Set: %f, Steering Set: %f \r\n", speed, steering); |
balsamfir | 15:caa5a93a31d7 | 82 | pc.printf("Left Motor Set: %f, Right Motor Set: %f \n\r\r\n\r\n", leftMotor, rightMotor); |
balsamfir | 2:2bc519e14bae | 83 | Thread::wait(500); // Go to sleep for 500 ms |
balsamfir | 2:2bc519e14bae | 84 | } |
balsamfir | 2:2bc519e14bae | 85 | } |
balsamfir | 2:2bc519e14bae | 86 | |
balsamfir | 2:2bc519e14bae | 87 | void ManualControl(void) { |
balsamfir | 15:caa5a93a31d7 | 88 | motorPeriodicInt.attach(&MotorISR, MOTOR_PERIOD); |
balsamfir | 16:73db7ef2deb6 | 89 | float speedRate, steeringRate; |
balsamfir | 16:73db7ef2deb6 | 90 | |
balsamfir | 15:caa5a93a31d7 | 91 | while (1) { |
balsamfir | 15:caa5a93a31d7 | 92 | osTimerStart(oneShotId, 2000); // Start or restart the watchdog timer interrupt and set to 2000ms. |
balsamfir | 16:73db7ef2deb6 | 93 | if (bt.readable()) { |
balsamfir | 16:73db7ef2deb6 | 94 | bt.scanf("%f :: %f", &speedRate, &steeringRate); |
balsamfir | 16:73db7ef2deb6 | 95 | //flushBuffer() |
balsamfir | 16:73db7ef2deb6 | 96 | |
balsamfir | 16:73db7ef2deb6 | 97 | speed = SPEED_MAX * speedRate; |
balsamfir | 16:73db7ef2deb6 | 98 | steering = (SPEED_MAX * steeringRate)/3; |
balsamfir | 16:73db7ef2deb6 | 99 | pc.printf("speed: %f, steering: %f \r\n", speedRate, steeringRate); |
balsamfir | 15:caa5a93a31d7 | 100 | |
balsamfir | 16:73db7ef2deb6 | 101 | bt.putc('~'); |
balsamfir | 15:caa5a93a31d7 | 102 | } |
balsamfir | 15:caa5a93a31d7 | 103 | |
balsamfir | 15:caa5a93a31d7 | 104 | osMutexWait(motorMutex, osWaitForever); |
balsamfir | 15:caa5a93a31d7 | 105 | leftMotor = speed - steering; |
balsamfir | 15:caa5a93a31d7 | 106 | rightMotor = speed + steering; |
balsamfir | 15:caa5a93a31d7 | 107 | osMutexRelease(motorMutex); |
balsamfir | 15:caa5a93a31d7 | 108 | } |
balsamfir | 15:caa5a93a31d7 | 109 | } |
balsamfir | 2:2bc519e14bae | 110 | |
balsamfir | 15:caa5a93a31d7 | 111 | void Tunning(void) { |
balsamfir | 15:caa5a93a31d7 | 112 | char key; |
balsamfir | 15:caa5a93a31d7 | 113 | float increment = 1; |
balsamfir | 15:caa5a93a31d7 | 114 | motorPeriodicInt.attach(&MotorISR, MOTOR_PERIOD); |
balsamfir | 15:caa5a93a31d7 | 115 | sensorPeriodicInt.attach(&NavigationISR, NAVIGATION_PERIOD); |
balsamfir | 15:caa5a93a31d7 | 116 | |
balsamfir | 15:caa5a93a31d7 | 117 | while (1) { |
balsamfir | 15:caa5a93a31d7 | 118 | osTimerStart(oneShotId, 2000); // Start or restart the watchdog timer interrupt and set to 2000ms. |
balsamfir | 15:caa5a93a31d7 | 119 | if (pc.readable()) { |
balsamfir | 15:caa5a93a31d7 | 120 | key = pc.getc(); |
balsamfir | 15:caa5a93a31d7 | 121 | if (key == 'q') { |
balsamfir | 15:caa5a93a31d7 | 122 | ShutDown(); |
balsamfir | 15:caa5a93a31d7 | 123 | return; |
balsamfir | 15:caa5a93a31d7 | 124 | } |
balsamfir | 15:caa5a93a31d7 | 125 | UpdateGains(key, &increment); |
balsamfir | 15:caa5a93a31d7 | 126 | pc.printf("Increment \t %f \r\n", increment); |
balsamfir | 15:caa5a93a31d7 | 127 | pc.printf("Motor \t KP: %f, KI: %f \r\n", leftMotorPI.kP, leftMotorPI.kI); |
balsamfir | 15:caa5a93a31d7 | 128 | pc.printf("Steering \t KP: %f, KI: %f \r\n", xPI.kP, xPI.kI); |
balsamfir | 15:caa5a93a31d7 | 129 | pc.printf("Speed \t KP: %f, KI: %f \r\n\r\n\r\n", heightPI.kP, heightPI.kI); |
balsamfir | 15:caa5a93a31d7 | 130 | } |
balsamfir | 15:caa5a93a31d7 | 131 | Thread::wait(500); // Go to sleep for 500 ms |
balsamfir | 15:caa5a93a31d7 | 132 | } |
balsamfir | 2:2bc519e14bae | 133 | } |
balsamfir | 2:2bc519e14bae | 134 | |
balsamfir | 14:2d609d465f00 | 135 | void ShutDown(void) { |
balsamfir | 15:caa5a93a31d7 | 136 | motorPeriodicInt.detach(); |
balsamfir | 15:caa5a93a31d7 | 137 | sensorPeriodicInt.detach(); |
balsamfir | 14:2d609d465f00 | 138 | osThreadTerminate(navigationId); |
balsamfir | 14:2d609d465f00 | 139 | osThreadTerminate(motorId); |
balsamfir | 14:2d609d465f00 | 140 | leftPwm.pulsewidth(0); |
balsamfir | 14:2d609d465f00 | 141 | rightPwm.pulsewidth(0); |
balsamfir | 14:2d609d465f00 | 142 | } |
balsamfir | 14:2d609d465f00 | 143 | |
balsamfir | 3:dfb6733ae397 | 144 | |
balsamfir | 3:dfb6733ae397 | 145 | // Threads |
balsamfir | 3:dfb6733ae397 | 146 | // ---------------------------------------------------------------- |
balsamfir | 3:dfb6733ae397 | 147 | void NavigationThread(void const *argument) { |
balsamfir | 3:dfb6733ae397 | 148 | int count; |
balsamfir | 14:2d609d465f00 | 149 | int missingCount; |
balsamfir | 3:dfb6733ae397 | 150 | |
balsamfir | 3:dfb6733ae397 | 151 | while (1) { |
balsamfir | 3:dfb6733ae397 | 152 | osSignalWait(navigationSignal, osWaitForever); // Go to sleep until navigation signal is set high by ISR |
balsamfir | 3:dfb6733ae397 | 153 | |
balsamfir | 3:dfb6733ae397 | 154 | count = pixy.getBlocks(1); |
balsamfir | 3:dfb6733ae397 | 155 | |
balsamfir | 3:dfb6733ae397 | 156 | // If target returned |
balsamfir | 3:dfb6733ae397 | 157 | if (count && (pixy.blocks[0].signature == TARGET_DECIMAL)) { |
balsamfir | 14:2d609d465f00 | 158 | missingCount = 0; |
balsamfir | 6:52686c25e4af | 159 | height = pixy.blocks[0].height; //use this for now |
balsamfir | 6:52686c25e4af | 160 | x = pixy.blocks[0].x; |
balsamfir | 5:f655435d0782 | 161 | |
balsamfir | 6:52686c25e4af | 162 | speed = heightPI.Run(HEIGHT_SETPOINT-height, SPEED_MAX); |
balsamfir | 7:5ef312aa2678 | 163 | steering = xPI.Run(X_SETPOINT-x, SPEED_MAX); |
balsamfir | 3:dfb6733ae397 | 164 | |
balsamfir | 3:dfb6733ae397 | 165 | // Give setpoints to MotorThread |
balsamfir | 3:dfb6733ae397 | 166 | osMutexWait(motorMutex, osWaitForever); |
balsamfir | 7:5ef312aa2678 | 167 | if (speed >= 0) { |
balsamfir | 7:5ef312aa2678 | 168 | leftMotor = speed - steering; |
balsamfir | 7:5ef312aa2678 | 169 | rightMotor = speed + steering; |
balsamfir | 7:5ef312aa2678 | 170 | } else { |
balsamfir | 7:5ef312aa2678 | 171 | leftMotor = speed - steering; |
balsamfir | 7:5ef312aa2678 | 172 | rightMotor = speed + steering; |
balsamfir | 7:5ef312aa2678 | 173 | } |
balsamfir | 3:dfb6733ae397 | 174 | osMutexRelease(motorMutex); |
balsamfir | 14:2d609d465f00 | 175 | } else { |
balsamfir | 15:caa5a93a31d7 | 176 | // When the robot can't see the target spin in last known direction |
balsamfir | 14:2d609d465f00 | 177 | if (missingCount >= 30) { |
balsamfir | 14:2d609d465f00 | 178 | osMutexWait(motorMutex, osWaitForever); |
balsamfir | 14:2d609d465f00 | 179 | leftMotor = (leftMotor > rightMotor)? SPEED_MAX/3 : -SPEED_MAX/3; |
balsamfir | 14:2d609d465f00 | 180 | rightMotor = (rightMotor > leftMotor)? SPEED_MAX/3 : -SPEED_MAX/3; |
balsamfir | 14:2d609d465f00 | 181 | osMutexRelease(motorMutex); |
balsamfir | 14:2d609d465f00 | 182 | } else { |
balsamfir | 14:2d609d465f00 | 183 | missingCount++; |
balsamfir | 14:2d609d465f00 | 184 | } |
balsamfir | 3:dfb6733ae397 | 185 | } |
balsamfir | 3:dfb6733ae397 | 186 | } |
balsamfir | 3:dfb6733ae397 | 187 | } |
balsamfir | 3:dfb6733ae397 | 188 | |
balsamfir | 3:dfb6733ae397 | 189 | void MotorThread(void const *argument) { |
balsamfir | 3:dfb6733ae397 | 190 | float leftSet, rightSet, angVel; |
balsamfir | 14:2d609d465f00 | 191 | float timeOnLeft, timeOnRight; |
balsamfir | 14:2d609d465f00 | 192 | short dP, dt; |
balsamfir | 3:dfb6733ae397 | 193 | |
balsamfir | 3:dfb6733ae397 | 194 | while (1) { |
balsamfir | 3:dfb6733ae397 | 195 | osSignalWait(motorSignal, osWaitForever); // Go to sleep until motor signal is set high by ISR |
balsamfir | 3:dfb6733ae397 | 196 | led2= !led2; // Alive status - led2 toggles each time MotorControlThread is signaled. |
balsamfir | 3:dfb6733ae397 | 197 | |
balsamfir | 3:dfb6733ae397 | 198 | // Get setpoints from navigation |
balsamfir | 3:dfb6733ae397 | 199 | osMutexWait(motorMutex, osWaitForever); |
balsamfir | 3:dfb6733ae397 | 200 | leftSet = leftMotor; |
balsamfir | 3:dfb6733ae397 | 201 | rightSet = rightMotor; |
balsamfir | 3:dfb6733ae397 | 202 | osMutexRelease(motorMutex); |
balsamfir | 3:dfb6733ae397 | 203 | |
balsamfir | 3:dfb6733ae397 | 204 | // Run PI control on left motor |
balsamfir | 3:dfb6733ae397 | 205 | dP = deSpi.write(0); |
balsamfir | 3:dfb6733ae397 | 206 | dt = deSpi.write(0); |
balsamfir | 3:dfb6733ae397 | 207 | angVel = QE2RadsPerSec(dP, dt); |
balsamfir | 6:52686c25e4af | 208 | timeOnLeft = leftMotorPI.Run(leftSet-angVel, PWM_PERIOD); |
balsamfir | 3:dfb6733ae397 | 209 | |
balsamfir | 3:dfb6733ae397 | 210 | // Run PI control on right motor |
balsamfir | 14:2d609d465f00 | 211 | dP = deSpi.write(0); |
balsamfir | 14:2d609d465f00 | 212 | dt = deSpi.write(0); |
balsamfir | 14:2d609d465f00 | 213 | angVel = -QE2RadsPerSec(dP, dt); // motors have opposite orientations |
balsamfir | 7:5ef312aa2678 | 214 | timeOnRight = rightMotorPI.Run(rightSet-angVel, PWM_PERIOD); // |
balsamfir | 3:dfb6733ae397 | 215 | |
balsamfir | 7:5ef312aa2678 | 216 | // Output new PWM and direction |
balsamfir | 6:52686c25e4af | 217 | if (timeOnLeft >= 0) leftDir = 1; |
balsamfir | 3:dfb6733ae397 | 218 | else leftDir = 0; |
balsamfir | 3:dfb6733ae397 | 219 | |
balsamfir | 6:52686c25e4af | 220 | if (timeOnRight >= 0) rightDir = 0; |
balsamfir | 3:dfb6733ae397 | 221 | else rightDir = 1; |
balsamfir | 3:dfb6733ae397 | 222 | |
balsamfir | 6:52686c25e4af | 223 | leftPwm.pulsewidth(fabs(timeOnLeft)); |
balsamfir | 6:52686c25e4af | 224 | rightPwm.pulsewidth(fabs(timeOnRight)); |
balsamfir | 3:dfb6733ae397 | 225 | } |
balsamfir | 3:dfb6733ae397 | 226 | } |
balsamfir | 3:dfb6733ae397 | 227 | |
balsamfir | 3:dfb6733ae397 | 228 | |
balsamfir | 3:dfb6733ae397 | 229 | // Interrupt Service Routines |
balsamfir | 3:dfb6733ae397 | 230 | // ---------------------------------------------------------------- |
balsamfir | 3:dfb6733ae397 | 231 | |
balsamfir | 3:dfb6733ae397 | 232 | // TODO: Shutdown system |
balsamfir | 3:dfb6733ae397 | 233 | void WatchdogISR(void const *n) { |
balsamfir | 3:dfb6733ae397 | 234 | led3=1; // Activated when the watchdog timer times out |
balsamfir | 3:dfb6733ae397 | 235 | } |
balsamfir | 3:dfb6733ae397 | 236 | |
balsamfir | 3:dfb6733ae397 | 237 | void MotorISR(void) { |
balsamfir | 3:dfb6733ae397 | 238 | osSignalSet(motorId,0x1); // Activate the signal, MotorControl, with each periodic timer interrupt |
balsamfir | 3:dfb6733ae397 | 239 | } |
balsamfir | 3:dfb6733ae397 | 240 | |
balsamfir | 3:dfb6733ae397 | 241 | void NavigationISR(void) { |
balsamfir | 3:dfb6733ae397 | 242 | osSignalSet(navigationId,0x1); // Activate the signal, SensorControl, with each periodic timer interrupt |
balsamfir | 3:dfb6733ae397 | 243 | } |
balsamfir | 3:dfb6733ae397 | 244 | |
balsamfir | 3:dfb6733ae397 | 245 | // TODO: Shutdown system |
balsamfir | 3:dfb6733ae397 | 246 | void CollisionISR(void) { |
balsamfir | 3:dfb6733ae397 | 247 | led4 = 1; // Activated on collision |
balsamfir | 14:2d609d465f00 | 248 | |
balsamfir | 14:2d609d465f00 | 249 | } |
balsamfir | 14:2d609d465f00 | 250 | |
balsamfir | 14:2d609d465f00 | 251 | // Nasty case function to update gain values |
balsamfir | 14:2d609d465f00 | 252 | void UpdateGains(char key, float *increment) { |
balsamfir | 14:2d609d465f00 | 253 | switch(key) { |
balsamfir | 14:2d609d465f00 | 254 | case 'e': |
balsamfir | 14:2d609d465f00 | 255 | leftMotorPI.kP = leftMotorPI.kP + *increment; |
balsamfir | 14:2d609d465f00 | 256 | break; |
balsamfir | 14:2d609d465f00 | 257 | case 'd': |
balsamfir | 14:2d609d465f00 | 258 | leftMotorPI.kP = leftMotorPI.kP - *increment; |
balsamfir | 14:2d609d465f00 | 259 | break; |
balsamfir | 14:2d609d465f00 | 260 | case 'r': |
balsamfir | 14:2d609d465f00 | 261 | leftMotorPI.kI = leftMotorPI.kI + *increment; |
balsamfir | 14:2d609d465f00 | 262 | break; |
balsamfir | 14:2d609d465f00 | 263 | case 'f': |
balsamfir | 14:2d609d465f00 | 264 | leftMotorPI.kI = leftMotorPI.kI - *increment; |
balsamfir | 14:2d609d465f00 | 265 | break; |
balsamfir | 14:2d609d465f00 | 266 | case 't': |
balsamfir | 14:2d609d465f00 | 267 | xPI.kP = xPI.kP + *increment; |
balsamfir | 14:2d609d465f00 | 268 | break; |
balsamfir | 14:2d609d465f00 | 269 | case 'g': |
balsamfir | 14:2d609d465f00 | 270 | xPI.kP = xPI.kP - *increment; |
balsamfir | 14:2d609d465f00 | 271 | break; |
balsamfir | 14:2d609d465f00 | 272 | case 'y': |
balsamfir | 14:2d609d465f00 | 273 | xPI.kI = xPI.kI + *increment; |
balsamfir | 14:2d609d465f00 | 274 | break; |
balsamfir | 14:2d609d465f00 | 275 | case 'h': |
balsamfir | 14:2d609d465f00 | 276 | xPI.kI = xPI.kI - *increment; |
balsamfir | 14:2d609d465f00 | 277 | break; |
balsamfir | 14:2d609d465f00 | 278 | case 'u': |
balsamfir | 14:2d609d465f00 | 279 | heightPI.kP = heightPI.kP + *increment; |
balsamfir | 14:2d609d465f00 | 280 | break; |
balsamfir | 14:2d609d465f00 | 281 | case 'j': |
balsamfir | 14:2d609d465f00 | 282 | heightPI.kP = heightPI.kP - *increment; |
balsamfir | 14:2d609d465f00 | 283 | break; |
balsamfir | 14:2d609d465f00 | 284 | case 'i': |
balsamfir | 14:2d609d465f00 | 285 | heightPI.kI = heightPI.kI + *increment; |
balsamfir | 14:2d609d465f00 | 286 | break; |
balsamfir | 14:2d609d465f00 | 287 | case 'k': |
balsamfir | 14:2d609d465f00 | 288 | heightPI.kI = heightPI.kI - *increment; |
balsamfir | 14:2d609d465f00 | 289 | break; |
balsamfir | 14:2d609d465f00 | 290 | case 'o': |
balsamfir | 14:2d609d465f00 | 291 | *increment = *increment * 10; |
balsamfir | 14:2d609d465f00 | 292 | break; |
balsamfir | 14:2d609d465f00 | 293 | case 'l': |
balsamfir | 14:2d609d465f00 | 294 | *increment = *increment / 10; |
balsamfir | 14:2d609d465f00 | 295 | break; |
balsamfir | 14:2d609d465f00 | 296 | } |
balsamfir | 2:2bc519e14bae | 297 | } |