| Wheelchair Control MCU firmware

- Main Page
- Modules
- Data Structures
- Files

- File List
- Globals

All Data Structures Files Functions Variables Enumerations Groups Pages
WheelchairControlMain.c
Go to the documentation of this file.
1
20 //Includes
21 #include "WheelchairControlMain.h"
22
23 //Debug mode warning
24 #ifdef DEBUG
25 #warning "DEBUG defined!"
26 #endif
27
28 t_ConfigStruct ConfigStructRAM;
29 t_ConfigStruct EEMEM ConfigStructEEPROM;
30 t_ISRsSharedStruct ISRsSharedStruct;
32 //*******************************************************************************************
33 //*******************************************************************************************
34 //*********************************** MAIN **************************************************
35 //*******************************************************************************************
36 //*******************************************************************************************
40 int16_t main(void)
41 {
42 InitializeMCU(); //Init all
43
44 BLINK_LEDS_MS(200); //Blinks LEDs to symbolize successful initialization and system start
45
46 while(TRUE) //Main loop - calculate odometry and regulators' outputs when no ISRs are being run
47 {
48 CalculateOdometry();
49
50 RunRegulators();
51 }
52
53 return TRUE;
54 }
55
56 //*******************************************************************************************
57 //*******************************************************************************************
58 //*************************** MAIN SUBROUTINES **********************************************
59 //*******************************************************************************************
60 //*******************************************************************************************
61
62 void InitializeMCU(void) //Initializes interrupts and peripherals
63 {
64 WATCHDOG_INITIALIZE(); //Watchdog initialize (reset interval aapr. 0.5s)
65
66 Init_ISRsSharedstruct(&ISRsSharedStruct); //Initializes inter-ISRs shared structure
67
68 EEPROM_LoadVariables(); //load variables from EEPROM to RAM
69
70 AC_Disable(); //disable analog comparator
71
72 ADC_Init(); //initialize A/D converter
73
74 PORTS_Init(); //initialize I/O port A - D (input/output, pull-ups etc.)
75
76 UART_Init(); //initialize UART
77
78 I2C_Init(); //initialize I2C
79
80 SRF08_Write(ConfigStructRAM.SRF08_Front,0x01, SRF08_GAIN); //set front sensor gain
81 SRF08_Write(ConfigStructRAM.SRF08_Front,0x02, (SRF08_RANGE/43)-1); //set front sensor range (in mm)
82
83 SRF08_Write(ConfigStructRAM.SRF08_Rear,0x01, SRF08_GAIN); //set rear sensor gain
84 SRF08_Write(ConfigStructRAM.SRF08_Rear,0x02, (SRF08_RANGE/43)-1); //set rear sensor range (in mm)
85
86 //Optionally set first safety SRF08 sensor gain
87 if(ConfigStructRAM.SRF08_SafetyFirst >= SRF08_LOWEST_ADDR && ConfigStructRAM.SRF08_SafetyFirst <= SRF08_HIGHEST_ADDR)
88 {
89 SRF08_Write(ConfigStructRAM.SRF08_SafetyFirst,0x01, SRF08_GAIN); //Set sensor gain
90 SRF08_Write(ConfigStructRAM.SRF08_SafetyFirst,0x02, (SRF08_RANGE/43)-1); //Set sensor range (in mm)
91 }
92
93 //Optionally set second safety SRF08 sensor gain
94 if(ConfigStructRAM.SRF08_SafetySecond >= SRF08_LOWEST_ADDR && ConfigStructRAM.SRF08_SafetySecond <= SRF08_HIGHEST_ADDR)
95 {
96 SRF08_Write(ConfigStructRAM.SRF08_SafetySecond,0x01, SRF08_GAIN); //Set sensor gain
97 SRF08_Write(ConfigStructRAM.SRF08_SafetySecond,0x02, (SRF08_RANGE/43)-1); //Set sensor range (in mm)
98 }
99
100 TIMER0_Init(); //initialize Timer0 (see this function description for TIMER0 parameters)
101 TIMER1_Init(); //initialize Timer1 (see this function description for TIMER1 parameters)
102 TIMER2_Init(); //initialize Timer2 (see this function description for TIMER2 parameters)
103
104 EXT_INT_Init(); //initialize external interrupts
105
106 GLOBAL_INTERRUPTS_ENABLE(); //enable interrupts
107 }
108
109 void CalculateOdometry(void) //Reads data from sensors and calculate position
110 {
111 if(ISRsSharedStruct.BufferOdoR || ISRsSharedStruct.BufferOdoL) //Calculate odometry only if buffers are not empty
112 {
113 int16_t trackM; //Track of midpoint
114 int32_t tempPhi; //Phi
115 int16_t trackR = (int16_t)(((int32_t)ISRsSharedStruct.BufferOdoR * (int32_t)ConfigStructRAM.RightWheelIncrement) / (int32_t)WHEEL_INCREMENT_SCALE); //Right wheel track [mm]
116 int16_t trackL = (int16_t)(((int32_t)ISRsSharedStruct.BufferOdoL * (int32_t)ConfigStructRAM.LeftWheelIncrement) / (int32_t)WHEEL_INCREMENT_SCALE); //Left wheel track [mm]
117
118 //Clear buffers
119 ISRsSharedStruct.BufferOdoR = 0;
120 ISRsSharedStruct.BufferOdoL = 0;
121
122 //Calculate track of midpoint, new phi (in mRads!)
123 trackM = (trackR + trackL) / 2;
124 tempPhi = (int32_t)ISRsSharedStruct.Phi + (int32_t)((((int32_t)trackR - (int32_t)trackL) * 100) / ((int32_t)ConfigStructRAM.Track));
125 tempPhi %= TWO_PI_MRAD;
126
127 if(tempPhi < 0) //Only positive values of phi are stored
128 {
129 tempPhi += TWO_PI_MRAD;
130 }
131
132
133 //Save odometry results atomically
134 TIMER2_OVF_INT_DISABLE();
135
136 ISRsSharedStruct.Phi = tempPhi;
137 ISRsSharedStruct.X += (int16_t)((double)trackM * cos(ISRsSharedStruct.Phi / 1000.0));
138 ISRsSharedStruct.Y += (int16_t)((double)trackM * sin(ISRsSharedStruct.Phi / 1000.0));
139
140 TIMER2_OVF_INT_ENABLE();
141 }
142 }
143
144 void RunRegulators(void) //Regulators (wall following and position regulator)
145 {
146 if (ISRsSharedStruct.EnablePositionRegulator) //Position regulator enabled, run it
147 {
148 int32_t tempSpR, tempSpL; //Temporary variables to calculate speeds
149 int32_t vTemp, wTemp; //Forward and angular speed [cm/s],[mRad/s]
150 double errDist, errAngle; //Error of distance [mm] and angle [mRad]
151
152 if(ISRsSharedStruct.RegulateAngleOnly) //Angular regulation enabled
153 {
154 //Regulate angle only - angle value is stored in the variable
155 errAngle = (double)((ISRsSharedStruct.RegulateAngleOnly / 1000.0) - (ISRsSharedStruct.Phi / 1000.0));
156
157 //Normalize to <-pi;pi> range
158 errAngle = errAngle - (2 * M_PI * floor((errAngle + M_PI) / (2 * M_PI)));
159
160 //Stop forward movement
161 vTemp = 0;
162
163 //Calculate angular speed
164 if (fabs(errAngle) <= (ConfigStructRAM.RegGamma / 10.0))
165 {
166 wTemp = (int32_t)(WTEMP_SCALE * (ConfigStructRAM.RegAngleGain / 10.0) * errAngle);
167 }
168 else
169 {
170 wTemp = (int32_t)(WTEMP_SCALE * (ConfigStructRAM.RegAngleGain / 10.0) * (ConfigStructRAM.RegGamma / 10.0) * (errAngle > 0 ? 1.0 : -1.0));
171 }
172
173 //Target angle reached, disable regulator
174 if(fabs(errAngle) <= REG_ANGLE_EPSILON)
175 ISRsSharedStruct.EnablePositionRegulator = FALSE;
176 }
177 else
178 {
179 //Calculate distance error
180 errDist = sqrt(square( (double)(ISRsSharedStruct.Xdes - ISRsSharedStruct.X) ) + square( (double)(ISRsSharedStruct.Ydes - ISRsSharedStruct.Y) ));
181
182 if ( (errDist > ConfigStructRAM.RegEpsilon)) //If target has not been reached yet
183 {
184 //Calculate angle error (atan2 returns value in range <-pi;+pi>)
185 errAngle = atan2((ISRsSharedStruct.Ydes - ISRsSharedStruct.Y),(ISRsSharedStruct.Xdes - ISRsSharedStruct.X)) - (ISRsSharedStruct.Phi / 1000.0);
186
187 //Normalize to <-pi;pi> range
188 errAngle = errAngle - (2 * M_PI * floor((errAngle + M_PI) / (2 * M_PI)));
189
190 //Calculate speeds
191 if (fabs(errAngle) <= (ConfigStructRAM.RegGamma / 10.0))
192 {
193 //Treat forward speed as constant -> better performance
194 //vTemp = (int32_t)((ConfigStructRAM.RegPositionGain / 10.0) * (errDist / 10)); //CURRENTLY DISABLED
195 vTemp = (int32_t)ConfigStructRAM.RegSpeed;
196
197 wTemp = (int32_t)(WTEMP_SCALE * (ConfigStructRAM.RegAngleGain / 10.0) * errAngle);
198 }
199 else
200 {
201 vTemp = 0;
202
203 wTemp = (int32_t)(WTEMP_SCALE * (ConfigStructRAM.RegAngleGain / 10.0) * (ConfigStructRAM.RegGamma / 10.0) * (errAngle > 0 ? 1.0 : -1.0));
204 }
205 }
206 else
207 {
208 vTemp = 0;
209 wTemp = 0;
210
211 //Target reached, disable position regulator
212 ISRsSharedStruct.EnablePositionRegulator = FALSE;
213 }
214 }
215
216 //Saturate omega
217 //Disable saturation by setting OmegaThresh to 0
218 if (ConfigStructRAM.OmegaThresh != 0)
219 {
220 //Angular speed threshold
221 if ((wTemp * WTEMP_SCALE) > ((int32_t)ConfigStructRAM.OmegaThresh * WTEMP_SCALE))
222 {
223 wTemp = (int32_t)ConfigStructRAM.OmegaThresh * WTEMP_SCALE;
224 }
225
226 if ((wTemp * WTEMP_SCALE) < (-1 * WTEMP_SCALE * (int32_t)ConfigStructRAM.OmegaThresh))
227 {
228 wTemp = (int32_t)(-1* WTEMP_SCALE * (int32_t)ConfigStructRAM.OmegaThresh);
229 }
230 }
231
232 //Calculate wheels' speeds in cm/s
233 tempSpR = (int32_t)( (vTemp) + (((int32_t)wTemp*(int32_t)ConfigStructRAM.Track)/(2.0 * WTEMP_SCALE)));
234 tempSpL = (int32_t)( (vTemp) - (((int32_t)wTemp*(int32_t)ConfigStructRAM.Track)/(2.0 * WTEMP_SCALE)));
235
236 //Prevent overflow / underflow
237 if (tempSpR < -500)
238 {
239 tempSpR = -500;
240 }
241
242 if (tempSpR > 500)
243 {
244 tempSpR = 500;
245 }
246
247 if (tempSpL < -500)
248 {
249 tempSpL = -500;
250 }
251
252 if (tempSpL > 500)
253 {
254 tempSpL = 500;
255 }
256
257 tempSpR = ((tempSpR * 2 * (int32_t)ConfigStructRAM.ImpPerRoundR) / (20*(int32_t)ConfigStructRAM.CircumR)) + SPEED_OFFSET ; //desired speed in pulses per 50 ms = (des. speed in cm/s * (impulses per round x2))/(speed sample frequency*circumference) + offset
258 tempSpL = ((tempSpL * 2 * (int32_t)ConfigStructRAM.ImpPerRoundL) / (20*(int32_t)ConfigStructRAM.CircumL)) + SPEED_OFFSET ; //desired speed in pulses per 50 ms = (des. speed in cm/s * (impulses per round x2))/(speed sample frequency*circumference) + offset
259
260 TIMER2_OVF_INT_DISABLE(); //Write speeds atomically
261
262 ISRsSharedStruct.MomSpeedR = (uint8_t)tempSpR; //actualize speed of the right wheel
263 ISRsSharedStruct.MomSpeedL = (uint8_t)tempSpL; //actualize speed of the left wheel
264
265 TIMER2_OVF_INT_ENABLE();
266 }
267 else if (ISRsSharedStruct.EnableWallFollowing) //Wall following enabled, run it
268 {
269 int32_t tempSpR, tempSpL; //temporary variables to calculate speeds
270 int16_t omega, d, d1, d2, y0; //variables of position regulator (angular speed, front and rear distance, d1 - d2), current y position
271
272 TIMER2_OVF_INT_DISABLE(); //Temporarily disable timer2 interrupt to guarantee SRF08s' data consistency
273
274 //Model linearized for fi ~ 0°
275 d1 = (int16_t)(ISRsSharedStruct.SRF08_DistanceFront + ConfigStructRAM.Track/2); //front distance
276 d2 = (int16_t)(ISRsSharedStruct.SRF08_DistanceRear + ConfigStructRAM.Track/2 + ((int16_t)ConfigStructRAM.SRF08_RearCorrection - REAR_CORRECTION_OFFSET) ); //rear distance
277
278 TIMER2_OVF_INT_ENABLE(); //Reenable interrupt
279
280 d = (int16_t)(d1 - d2); //Deviance from zero angle
281
282 y0 = (d1+d2)/2; //Actual midpoint's y position
283
284 //Calculate angular speed ( omega = Kfi*d - Ky*(y0_des - y0) )
285 //Cast necessary to prevent signed and unsigned types collision
286 //Warning! This calculation is true only if sensor are placed on the left side of the wheelchair, otherwise omega must by multiplied by -1!!!
287 omega = (((int16_t)ISRsSharedStruct.Kfi * (int16_t)d) / 10 )
288 - (((int16_t)ISRsSharedStruct.Ky * ((int16_t)ISRsSharedStruct.y0_des - (int16_t)y0)) / 10 );
289
290 //Invert omega sign if steering wheel is on the left side
291 if(STEERING_WHEEL_LEFT)
292 {
293 omega *= (int16_t)(-1);
294 }
295
296 //Saturate omega
297 //Disable saturation by setting OmegaThresh to 0
298 if (ConfigStructRAM.OmegaThresh != 0) //Disable thresholding by setting OmegaThresh to 0
299 {
300 //Angular speed threshold
301 if (omega > ConfigStructRAM.OmegaThresh)
302 {
303 omega = (int16_t)ConfigStructRAM.OmegaThresh;
304 }
305
306 if (omega < (-1*ConfigStructRAM.OmegaThresh))
307 {
308 omega = (int16_t)(-1*ConfigStructRAM.OmegaThresh);
309 }
310 }
311
312 //Calculate wheels' speeds in cm/s
313 tempSpR = (int32_t)( (ISRsSharedStruct.V_des - SPEED_OFFSET) + (((int32_t)omega * OMEGA_SCALE * (int32_t)ConfigStructRAM.Track)/2.0));
314 tempSpL = (int32_t)( (ISRsSharedStruct.V_des - SPEED_OFFSET) - (((int32_t)omega * OMEGA_SCALE * (int32_t)ConfigStructRAM.Track)/2.0));
315
316 //Prevent overflow / underflow
317 if (tempSpR < -500)
318 {
319 tempSpR = -500;
320 }
321
322 if (tempSpR > 500)
323 {
324 tempSpR = 500;
325 }
326
327 if (tempSpL < -500)
328 {
329 tempSpL = -500;
330 }
331
332 if (tempSpL > 500)
333 {
334 tempSpL = 500;
335 }
336
337 tempSpR = ((tempSpR * 2 * (int32_t)ConfigStructRAM.ImpPerRoundR) / (20 * (int32_t)ConfigStructRAM.CircumR)) + SPEED_OFFSET ; //desired speed in pulses per 50 ms = (des. speed in cm/s * (impulses per round x2))/(speed sample frequency*circumference) + offset
338 tempSpL = ((tempSpL * 2 * (int32_t)ConfigStructRAM.ImpPerRoundL) / (20 * (int32_t)ConfigStructRAM.CircumL)) + SPEED_OFFSET ; //desired speed in pulses per 50 ms = (des. speed in cm/s * (impulses per round x2))/(speed sample frequency*circumference) + offset
339
340 //Check whether specified distance has not been reached
341 if(ISRsSharedStruct.Xdes && (fabs(ISRsSharedStruct.X) >= (fabs(ISRsSharedStruct.Xdes))))
342 {
343 //If Xdes is set, it acts as distance limit for wall-following algorithm
344 //If traveled distance is greater than Xdes, wall following alorithm is stopped
345
346 tempSpR = SPEED_OFFSET;
347 tempSpL = SPEED_OFFSET;
348
349 ISRsSharedStruct.EnableWallFollowing = FALSE;
350 }
351
352 TIMER2_OVF_INT_DISABLE(); //Write speeds atomically
353
354 ISRsSharedStruct.MomSpeedR = (uint8_t)tempSpR; //actualize speed of the right wheel
355 ISRsSharedStruct.MomSpeedL = (uint8_t)tempSpL; //actualize speed of the left wheel
356
357 TIMER2_OVF_INT_ENABLE();
358 }
359 }
360
361 //*******************************************************************************************
362 //*******************************************************************************************
363 //*************************** EEPROM RW ROUTINES ********************************************
364 //*******************************************************************************************
365 //*******************************************************************************************
366
367 void EEPROM_LoadVariables(void) //loads variables stored in EEPROM to RAM
368 {
369 ConfigStructRAM.SRF08_Front = eeprom_read_byte((uint8_t*)&ConfigStructEEPROM.SRF08_Front); //read address of front SRF08 sensor from EEPROM
370 ConfigStructRAM.SRF08_Rear = eeprom_read_byte((uint8_t*)&ConfigStructEEPROM.SRF08_Rear); //read address of rear SRF08 sensor from EEPROM
371
372 ConfigStructRAM.SRF08_SafetyFirst = eeprom_read_byte((uint8_t*)&ConfigStructEEPROM.SRF08_SafetyFirst); //Read address of first safety sensor
373 ConfigStructRAM.SRF08_SafetySecond = eeprom_read_byte((uint8_t*)&ConfigStructEEPROM.SRF08_SafetySecond); //Read address of first safety sensor
374
375 ConfigStructRAM.ImpPerRoundR = eeprom_read_byte((uint8_t*)&ConfigStructEEPROM.ImpPerRoundR); //read imp. per round of the right encoder
376 ConfigStructRAM.ImpPerRoundL = eeprom_read_byte((uint8_t*)&ConfigStructEEPROM.ImpPerRoundL); //read imp. per round of the left encoder
377
378 ConfigStructRAM.MaxAcc = eeprom_read_byte((uint8_t*)&ConfigStructEEPROM.MaxAcc); //read max. acceleration
379
380 ConfigStructRAM.Kp = eeprom_read_byte((uint8_t*)&ConfigStructEEPROM.Kp); //read Kp coefficient of the regulator
381 ConfigStructRAM.Ki = eeprom_read_byte((uint8_t*)&ConfigStructEEPROM.Ki); //read Kp coefficient of the regulator
382 ConfigStructRAM.Kd = eeprom_read_byte((uint8_t*)&ConfigStructEEPROM.Kd); //read Kp coefficient of the regulator
383
384 ConfigStructRAM.RegErrorFilter = eeprom_read_byte((uint8_t*)&ConfigStructEEPROM.RegErrorFilter); //read regulator error threshold
385
386 ConfigStructRAM.CircumR = eeprom_read_word((uint16_t*)&ConfigStructEEPROM.CircumR); //read right wheel circumference
387 ConfigStructRAM.CircumL = eeprom_read_word((uint16_t*)&ConfigStructEEPROM.CircumL); //read left wheel circumference
388
389 ConfigStructRAM.Track = eeprom_read_word((uint16_t*)&ConfigStructEEPROM.Track); //read track of wheels
390
391 ConfigStructRAM.Omega0 = eeprom_read_byte((uint8_t*)&ConfigStructEEPROM.Omega0); //read omega0
392 ConfigStructRAM.L = eeprom_read_byte((uint8_t*)&ConfigStructEEPROM.L); //read L
393 ConfigStructRAM.Ksi = eeprom_read_byte((uint8_t*)&ConfigStructEEPROM.Ksi); //read Ksi
394
395 ConfigStructRAM.OmegaThresh = eeprom_read_byte((uint8_t*)&ConfigStructEEPROM.OmegaThresh); //read omega threshold
396
397 ConfigStructRAM.SRF08_RearCorrection = eeprom_read_byte((uint8_t*)&ConfigStructEEPROM.SRF08_RearCorrection); //Read correction factor
398
399 ConfigStructRAM.RightWheelIncrement = (uint8_t)( ((uint32_t)ConfigStructRAM.CircumR * (uint32_t)10 * (uint32_t)WHEEL_INCREMENT_SCALE) / (uint32_t)((uint32_t)ConfigStructRAM.ImpPerRoundR * (uint32_t)2) ); //Right wheel track increment [mm]
400 ConfigStructRAM.LeftWheelIncrement = (uint8_t)( ((uint32_t)ConfigStructRAM.CircumL * (uint32_t)10 * (uint32_t)WHEEL_INCREMENT_SCALE) /(uint32_t)((uint32_t)ConfigStructRAM.ImpPerRoundL * (uint32_t)2) ); //Left wheel track increment [mm]
401
402 ConfigStructRAM.RegGamma = eeprom_read_byte((uint8_t*)&ConfigStructEEPROM.RegGamma); //Read regulator's gamma
403 ConfigStructRAM.RegEpsilon = eeprom_read_byte((uint8_t*)&ConfigStructEEPROM.RegEpsilon); //Read regulator's epsilon
404 ConfigStructRAM.RegAngleGain = eeprom_read_byte((uint8_t*)&ConfigStructEEPROM.RegAngleGain); //Read regulator's angle gain
405 ConfigStructRAM.RegPositionGain = eeprom_read_byte((uint8_t*)&ConfigStructEEPROM.RegPositionGain); //Read regulator's position gain
406 ConfigStructRAM.RegSpeed = eeprom_read_byte((uint8_t*)&ConfigStructEEPROM.RegSpeed); //Read regulator's position gain
407 }
408
409 void EEPROM_WriteVariable(uint8_t* address, uint8_t data) //writes byte to EEPROM
410 {
411 eeprom_update_byte(address, data); //write the value to EEPROM
412
413 EEPROM_LoadVariables(); //reloads variables from EEPROM to RAM
414 }
415
416 void EEPROM_WriteWord(uint16_t* address, uint16_t data) //writes word (2 bytes) to EEPROM
417 {
418 eeprom_update_word(address, data); //write the value to EEPROM
419
420 EEPROM_LoadVariables(); //reloads variables from EEPROM to RAM
421 }
422
423 //*******************************************************************************************
424 //*******************************************************************************************
425 //*************************** INTERRUPT SERVICE ROUTINES ************************************
426 //*******************************************************************************************
427 //*******************************************************************************************
428
432 ISR(USART_RXC_vect)
433 {
434 static uint8_t UARTReceivedData[4]; //Buffer for received data
435
436 TIMER_0_RESET(); //Reset timer & prescaler
437 ISRsSharedStruct.Timer0Prescaler = 0;
438
439 UARTReceivedData[ISRsSharedStruct.UARTRxIndex++] = UDR; //read data
440
441 if(ISRsSharedStruct.UARTRxIndex >= RX_PACKET_LENGTH) //whole packet has been received
442 {
443 uint8_t checksum = (uint8_t)(UARTReceivedData[COMMAND_IDENTIFIER] + UARTReceivedData[DATA1] + UARTReceivedData[DATA2]);
444
445 if(checksum == UARTReceivedData[RX_CHECKSUM_BYTE]) //if checksum is correct
446 {
447 PORTA |= LED_IN; //turn on LED_IN (PORTA.1) status to symbolize received packet
448
449 ISRsSharedStruct.UARTWatchdog = 0; //reset the watchdog counter
450
451 switch(UARTReceivedData[COMMAND_IDENTIFIER]) //analyze received data and perform the command
452 {
453 default: break; //unrecognized command
454
455 //A -> Odometry
456
457 case ODOMETRY_SET_X: //Set X coordinate
458
459 ISRsSharedStruct.X = (int16_t)(((int16_t)UARTReceivedData[DATA1] << 8) + UARTReceivedData[DATA2]);
460
461 break;
462
463 case ODOMETRY_SET_Y: //Set Y coordinate
464
465 ISRsSharedStruct.Y = (int16_t)(((int16_t)UARTReceivedData[DATA1] << 8) + UARTReceivedData[DATA2]);
466
467 break;
468
469 case ODOMETRY_SET_PHI: //Set Y coordinate
470
471 ISRsSharedStruct.Phi = (int16_t)(((int16_t)UARTReceivedData[DATA1] << 8) + UARTReceivedData[DATA2]);
472
473 break;
474
475
476 case ODOMETRY_RESET_ALL: //Reset all coordinates
477
478 ISRsSharedStruct.BufferOdoL = 0;
479 ISRsSharedStruct.BufferOdoR = 0;
480
481 ISRsSharedStruct.TotalImpulsesR = 0;
482 ISRsSharedStruct.TotalImpulsesL = 0;
483
484 ISRsSharedStruct.X = 0;
485 ISRsSharedStruct.Y = 0;
486 ISRsSharedStruct.Phi = 0;
487
488 break;
489
490
491 //C -> Motors control
492
493 case MOTORS_RIGHT_MOMENTUM: //right motor momentum (128 = 0, 255 = full forward, 0 = full backwards)
494
495 ISRsSharedStruct.EnablePositionRegulator = FALSE; //Disable position regulator
496
497 ISRsSharedStruct.EnableWallFollowing = FALSE; //disable wall following algorithm
498
499 ISRsSharedStruct.EnableSpRegR = FALSE; //disable speed regulator
500
501 ISRsSharedStruct.RegIntegratorR = 0; //reset the integrator
502
503 ISRsSharedStruct.MomSpeedR = UARTReceivedData[DATA2];
504
505 break;
506
507 case MOTORS_LEFT_MOMENTUM: //left motor momentum (128 = 0, 255 = full forward, 0 = full backwards)
508
509 ISRsSharedStruct.EnablePositionRegulator = FALSE;; //Disable position regulator
510
511 ISRsSharedStruct.EnableWallFollowing = FALSE; //disable wall following algorithm
512
513 ISRsSharedStruct.EnableSpRegL = FALSE; //disable speed regulator
514
515 ISRsSharedStruct.RegIntegratorL = 0; //reset the integrator
516
517 ISRsSharedStruct.MomSpeedL = UARTReceivedData[DATA2];
518
519 break;
520
521 case MOTORS_RIGHT_SPEED_CMPS: //right motor speed in cm/s (-500 to + 500 cm per s, offset +500 -> values 0 to 1000)
522 {
523 int32_t tempSp; //temp for speed calculation
524
525 ISRsSharedStruct.EnablePositionRegulator = FALSE; //Disable position regulator
526
527 ISRsSharedStruct.EnableWallFollowing = FALSE; //disable wall following algorithm
528
529 ISRsSharedStruct.EnableSpRegR = TRUE; //enable speed regulator
530
531 tempSp = (int32_t)((UARTReceivedData[DATA1]<<8) + UARTReceivedData[DATA2]); //calculate the speed in cm/s
532
533 if (tempSp < 0) //prevent underflow
534 {
535 tempSp = 0;
536 }
537
538 if (tempSp > 1000) //prevent overflow
539 {
540 tempSp = 1000;
541 }
542
543 tempSp -= 500; //substract the offset
544
545 tempSp = (((int32_t)tempSp * 2 * (int32_t)ConfigStructRAM.ImpPerRoundR) / (20*(int32_t)ConfigStructRAM.CircumR)) + SPEED_OFFSET; //desired speed in pulses per 50 ms = (des. speed in cm/s * impulses per roud)/(speed sample frequency*circumference) + offset
546
547 ISRsSharedStruct.MomSpeedR = (uint8_t)tempSp; //actualize the speed
548
549 break;
550
551 }
552
553 case MOTORS_LEFT_SPEED_CMPS: //left motor speed in cm/s (-500 to + 500 cm per s, offset +500 -> values 0 to 1000)
554 {
555 int32_t tempSp; //temp for speed calculation
556
557 ISRsSharedStruct.EnablePositionRegulator = FALSE; //Disable position regulator
558
559 ISRsSharedStruct.EnableWallFollowing = FALSE; //disable wall following algorithm
560
561 ISRsSharedStruct.EnableSpRegL = TRUE; //enable speed regulator
562
563 tempSp = (int32_t)((UARTReceivedData[DATA1]<<8) + UARTReceivedData[DATA2]); //calculate the speed in cm/s
564
565 if (tempSp < 0) //prevent underflow
566 {
567 tempSp = 0;
568 }
569
570 if (tempSp > 1000) //prevent overflow
571 {
572 tempSp = 1000;
573 }
574
575 tempSp -= 500; //substract the offset
576
577 tempSp = ((tempSp * 2 * (int32_t)ConfigStructRAM.ImpPerRoundL) / (20*(int32_t)ConfigStructRAM.CircumL)) + SPEED_OFFSET; //desired speed in pulses per 50 ms = (des. speed in cm/s * impulses per roud)/(speed sample frequency*circumference) + offset
578
579 ISRsSharedStruct.MomSpeedL = (uint8_t)tempSp; //actualize the speed
580
581 break;
582
583 }
584
585 case MOTORS_BOTH_MOMENTUM: //right and left motor momentum (128 = 0, 255 = full forward, 0 = full backwards)
586
587 ISRsSharedStruct.EnableWallFollowing = FALSE; //disable wall following algorithm
588
589 ISRsSharedStruct.EnablePositionRegulator = FALSE; //Disable position regulator
590
591 ISRsSharedStruct.EnableSpRegR = FALSE; //disable speed regulator
592 ISRsSharedStruct.EnableSpRegL = FALSE; //disable speed regulator
593
594 ISRsSharedStruct.RegIntegratorR = 0; //reset the integrator
595 ISRsSharedStruct.RegIntegratorL = 0; //reset the integrator
596
597 ISRsSharedStruct.MomSpeedR = UARTReceivedData[DATA1];
598 ISRsSharedStruct.MomSpeedL = UARTReceivedData[DATA2];
599
600 break;
601
602 case MOTORS_BOTH_SPEED_IMPPS: //right and left motor speed in impulses per 50ms ~ impps/20 (128 = 0, 255 = full forward, 0 = full backwards)
603
604 ISRsSharedStruct.EnableWallFollowing = FALSE; //disable wall following algorithm
605
606 ISRsSharedStruct.EnablePositionRegulator = FALSE; //Disable position regulator
607
608 ISRsSharedStruct.EnableSpRegR = TRUE; //enable speed regulator
609 ISRsSharedStruct.EnableSpRegL = TRUE; //enable speed regulator
610
611 ISRsSharedStruct.MomSpeedR = UARTReceivedData[DATA1];
612 ISRsSharedStruct.MomSpeedL = UARTReceivedData[DATA2];
613
614 break;
615
616 case MOTORS_WALL_FOLLOWING: //starts wall-following algorithm using desired speed V (cm/s) and y0 (cm)
617
618 ISRsSharedStruct.EnablePositionRegulator = FALSE; //Disable position regulator
619
620 ISRsSharedStruct.EnableWallFollowing = TRUE; //enable wall following algorithm
621
622 ISRsSharedStruct.EnableSpRegR = TRUE; //enable speed regulator
623 ISRsSharedStruct.EnableSpRegL = TRUE; //enable speed regulator
624
625 ISRsSharedStruct.V_des = UARTReceivedData[DATA1]; //desired speed in cm/s
626 ISRsSharedStruct.y0_des = UARTReceivedData[DATA2]; //desired distance from wall in cm
627
628 if(ISRsSharedStruct.V_des != SPEED_OFFSET) //offset 128 -> 128 is equal to zero speed
629 {
630 ISRsSharedStruct.Ky = (int16_t)( ( ((int16_t)(ConfigStructRAM.Omega0) * (int16_t)(ConfigStructRAM.Omega0) / 2) ) / ((int16_t)(ISRsSharedStruct.V_des - SPEED_OFFSET) ) ); //Ky = (omega0)^2 / V
631 }
632
633 ISRsSharedStruct.Kfi = (int16_t)( ( ((int16_t)ConfigStructRAM.Ksi) * ((int16_t)ConfigStructRAM.Omega0) ) / ((int16_t)ConfigStructRAM.L) ); //Kfi = (Ksi * omega0)/L
634
635 break;
636
637 case MOTORS_POSITION_REGULATOR: //Starts position regulation
638
639 ISRsSharedStruct.EnablePositionRegulator = TRUE; //Enable position regulator
640
641 ISRsSharedStruct.EnableWallFollowing = FALSE; //Disable wall following algorithm
642
643 ISRsSharedStruct.RegulateAngleOnly = (int16_t)(((int16_t)UARTReceivedData[DATA1] << 8) + UARTReceivedData[DATA2]); //Angle / speed + angle regulator
644
645 ISRsSharedStruct.EnableSpRegR = TRUE; //Enable speed regulator
646 ISRsSharedStruct.EnableSpRegL = TRUE; //Enable speed regulator
647
648 break;
649
650 //D - > Regulators
651
652 case REGULATORS_X_DESIRED: //Sets desired X coordinate for the position regulator
653
654 ISRsSharedStruct.Xdes = (int16_t)(((int16_t)UARTReceivedData[DATA1] << 8) + UARTReceivedData[DATA2]);
655
656 break;
657
658 case REGULATORS_Y_DESIRED: //Sets desired Y coordinate for the position regulator
659
660 ISRsSharedStruct.Ydes = (int16_t)(((int16_t)UARTReceivedData[DATA1] << 8) + UARTReceivedData[DATA2]);
661
662 break;
663
664 case REGULATORS_GAMMA: //Sets gamma (angle of view)
665
666 EEPROM_WriteVariable(&ConfigStructEEPROM.RegGamma, (uint8_t)UARTReceivedData[DATA2]);
667
668 break;
669
670 case REGULATORS_EPSILON: //Sets epsilon (acceptable error of target position)
671
672 EEPROM_WriteVariable(&ConfigStructEEPROM.RegEpsilon, (uint8_t)UARTReceivedData[DATA2]);
673
674 break;
675
676 case REGULATORS_ANGLE_GAIN: //Set gain of angle feedback loop
677
678 EEPROM_WriteVariable(&ConfigStructEEPROM.RegAngleGain, (uint8_t)UARTReceivedData[DATA2]);
679
680 break;
681
682 case REGULATORS_POSITION_GAIN: //Set gain of position feedback loop
683
684 EEPROM_WriteVariable(&ConfigStructEEPROM.RegPositionGain, (uint8_t)UARTReceivedData[DATA2]);
685
686 break;
687
688 case REGULATORS_SPEED: //Set constant forward speed in case variable speed regulation is forbidden
689
690 EEPROM_WriteVariable(&ConfigStructEEPROM.RegSpeed, (uint8_t)UARTReceivedData[DATA2]);
691
692 break;
693
694
695 //E - > Configuration
696
697 case CONFIGURATION_RIGHT_WHEEL_CIRCUM: //sets the right wheel circumference in cm
698
699 EEPROM_WriteWord(&ConfigStructEEPROM.CircumR, (UARTReceivedData[DATA1]<<8) + UARTReceivedData[DATA2]);
700
701 break;
702
703 case CONFIGURATION_LEFT_WHEEL_CIRCUM: //sets the left wheel circumference in cm
704
705 EEPROM_WriteWord(&ConfigStructEEPROM.CircumL, (UARTReceivedData[DATA1]<<8) + UARTReceivedData[DATA2]);
706
707 break;
708
709 case CONFIGURATION_TRACK: //sets track of wheels in cm
710
711 EEPROM_WriteWord(&ConfigStructEEPROM.Track, (UARTReceivedData[DATA1]<<8) + UARTReceivedData[DATA2]);
712
713 break;
714
715 case CONFIGURATION_IMP_PER_ROUND: //sets impulses per round
716
717 EEPROM_WriteVariable(&ConfigStructEEPROM.ImpPerRoundR, (uint8_t)UARTReceivedData[DATA1]);
718 EEPROM_WriteVariable(&ConfigStructEEPROM.ImpPerRoundL, (uint8_t)UARTReceivedData[DATA2]);
719
720 break;
721
722 case CONFIGURATION_SRF08_SAFETY_ADDRESSES: //Safety sensors addresses
723
724 EEPROM_WriteVariable(&ConfigStructEEPROM.SRF08_SafetyFirst, (uint8_t)UARTReceivedData[DATA1]);
725 EEPROM_WriteVariable(&ConfigStructEEPROM.SRF08_SafetySecond, (uint8_t)UARTReceivedData[DATA2]);
726
727
728 break;
729
730 case CONFIGURATION_MAX_ACCELERATION: //sets maximal acceleration
731
732 EEPROM_WriteVariable(&ConfigStructEEPROM.MaxAcc, (uint8_t)UARTReceivedData[DATA2]);
733
734 break;
735
736 case CONFIGURATION_KP: //sets Kp coefficient of the PSD regulator (must be multiplied by 10!!!)
737
738 EEPROM_WriteVariable(&ConfigStructEEPROM.Kp, (uint8_t)UARTReceivedData[DATA2]);
739
740 break;
741
742 case CONFIGURATION_KD: //sets Kd coefficient of the PSD regulator (must be multiplied by 10!!!)
743
744 EEPROM_WriteVariable(&ConfigStructEEPROM.Kd, (uint8_t)UARTReceivedData[DATA2]);
745
746 break;
747
748 case CONFIGURATION_KI: //sets Ki coefficient of the PSD regulator (must be multiplied by 10!!!)
749
750 EEPROM_WriteVariable(&ConfigStructEEPROM.Ki, (uint8_t)UARTReceivedData[DATA2]);
751
752 break;
753
754 case CONFIGURATION_SRF08_ADDRESSES: //sets addresses of front and rear SRF08 sensor
755
756 EEPROM_WriteVariable(&ConfigStructEEPROM.SRF08_Front, (uint8_t)UARTReceivedData[DATA1]);
757 EEPROM_WriteVariable(&ConfigStructEEPROM.SRF08_Rear, (uint8_t)UARTReceivedData[DATA2]);
758
759 break;
760
761 case CONFIGURATION_REG_ERR_THRESH: //sets regulator error threshold
762
763 EEPROM_WriteVariable(&ConfigStructEEPROM.RegErrorFilter, (uint8_t)UARTReceivedData[DATA2]);
764
765 break;
766
767 case CONFIGURATION_OMEGA_0: //sets the omega0 value (multiplied by 10!!!)
768
769 EEPROM_WriteVariable(&ConfigStructEEPROM.Omega0, (uint8_t)UARTReceivedData[DATA2]);
770
771 break;
772
773 case CONFIGURATION_L: //sets the L (distance between SRF08 sensors/2 in cm)
774
775 EEPROM_WriteVariable(&ConfigStructEEPROM.L, (uint8_t)UARTReceivedData[DATA2]);
776
777 break;
778
779 case CONFIGURATION_KSI: //sets the ksi value (multiplied by 10!!)
780
781 EEPROM_WriteVariable(&ConfigStructEEPROM.Ksi, (uint8_t)UARTReceivedData[DATA2]);
782
783 break;
784
785 case CONFIGURATION_OMEGA_THRESH: //sets the omegaThresh value
786
787 EEPROM_WriteVariable(&ConfigStructEEPROM.OmegaThresh, (uint8_t)UARTReceivedData[DATA2]);
788
789 break;
790
791 case CONFIGURATION_REAR_CORRECTION: //Sets rear sensor distance correction
792
793 EEPROM_WriteVariable(&ConfigStructEEPROM.SRF08_RearCorrection, (uint8_t)UARTReceivedData[DATA2]);
794
795 break;
796
797 //F -> Send data
798
799 case SEND_ODOMETRY: //Send odometry data
800 {
801 uint8_t UARTTransmitData[TX_PACKET_LENGTH]; //Data buffer
802
803 PORTA |= LED_OUT; //Turn on LED_OUT (PORTA.2) to symbolize outgoing packet
804
805 UARTTransmitData[0] = (uint8_t)(ISRsSharedStruct.X >> 8); //X coordinate [mm] - High byte
806 UARTTransmitData[1] = (uint8_t)(ISRsSharedStruct.X & 0x00FF);//X coordinate [mm] - Low byte
807
808 UARTTransmitData[2] = (uint8_t)(ISRsSharedStruct.Y >> 8); //Y coordinate [mm] - High byte
809 UARTTransmitData[3] = (uint8_t)(ISRsSharedStruct.Y & 0x00FF);//Y coordinate [mm] - Low byte
810
811 UARTTransmitData[4] = (uint8_t)(ISRsSharedStruct.Phi >> 8); //Phi [mRad] - High byte
812 UARTTransmitData[5] = (uint8_t)(ISRsSharedStruct.Phi & 0x00FF); //Phi [mRad] - Low byte
813
814 UARTTransmitData[6] = (uint8_t)ISRsSharedStruct.SRF08_DistanceFront; //Front distance measured by SRF08
815 UARTTransmitData[7] = (uint8_t)ISRsSharedStruct.SRF08_DistanceRear; //Rear distance measured by SRF08
816
817 UARTTransmitData[8] = (uint8_t)(ISRsSharedStruct.EnablePositionRegulator); //Send position regulator status
818 UARTTransmitData[9] = (uint8_t)(ISRsSharedStruct.EnableWallFollowing); //Send wall following regulator status
819
820 UARTTransmitData[10] = 0;
821
822 UARTTransmitData[TX_CHECKSUM_BYTE] = 0;
823
824 for(uint8_t i = 0; i < TX_CHECKSUM_BYTE; i++)
825 {
826 UARTTransmitData[TX_CHECKSUM_BYTE] += UARTTransmitData[i]; //calculate checksum
827
828 UART_SendByte((uint8_t)UARTTransmitData[i]); //send data via UART
829 }
830
831 UART_SendByte((uint8_t)UARTTransmitData[TX_CHECKSUM_BYTE]); //send checksum via UART
832
833 break;
834
835 }
836
837 case SEND_CONFIG_1: //send configuration data 1
838 {
839 uint8_t UARTTransmitData[TX_PACKET_LENGTH]; //Data buffer
840
841 PORTA |= LED_OUT; //Turn on LED_OUT (PORTA.2) to symbolize outgoing packet
842
843 UARTTransmitData[0] = eeprom_read_byte((uint8_t*)&ConfigStructEEPROM.ImpPerRoundR); //imp/round of the right wheel
844 UARTTransmitData[1] = eeprom_read_byte((uint8_t*)&ConfigStructEEPROM.ImpPerRoundL); //imp/round of the left wheel
845
846 UARTTransmitData[2] = (eeprom_read_word((uint16_t*)&ConfigStructEEPROM.CircumR)>>8); //right wheel circumference high byte
847 UARTTransmitData[3] = (eeprom_read_word((uint16_t*)&ConfigStructEEPROM.CircumR) & 0x00FF); //right wheel circumference low byte
848
849 UARTTransmitData[4] = (eeprom_read_word((uint16_t*)&ConfigStructEEPROM.CircumL)>>8); //left wheel circumference high byte
850 UARTTransmitData[5] = (eeprom_read_word((uint16_t*)&ConfigStructEEPROM.CircumL) & 0x00FF); //left wheel circumference low byte
851
852 UARTTransmitData[6] = (eeprom_read_word((uint16_t*)&ConfigStructEEPROM.Track)>>8); //track high byte
853 UARTTransmitData[7] = (eeprom_read_word((uint16_t*)&ConfigStructEEPROM.Track) & 0x00FF); //track low byte
854
855 UARTTransmitData[8] = eeprom_read_byte((uint8_t*)&ConfigStructEEPROM.SRF08_Front); //front SRF08 address
856 UARTTransmitData[9] = eeprom_read_byte((uint8_t*)&ConfigStructEEPROM.SRF08_Rear); //rear SRF08 address
857
858 UARTTransmitData[10] = (int8_t)(eeprom_read_byte((uint8_t*)&ConfigStructEEPROM.SRF08_RearCorrection) - 128); //rear correction factor
859
860 UARTTransmitData[TX_CHECKSUM_BYTE] = 0; //reset checksum
861
862 for(uint8_t i = 0; i < TX_CHECKSUM_BYTE; i++)
863 {
864 UARTTransmitData[TX_CHECKSUM_BYTE] += UARTTransmitData[i]; //calculate checksum
865
866 UART_SendByte((uint8_t)UARTTransmitData[i]); //send data via UART
867 }
868
869 UART_SendByte((uint8_t)UARTTransmitData[TX_CHECKSUM_BYTE]); //send checksum via UART
870
871 break;
872
873 }
874
875 case SEND_CONFIG_2: //send configuration data 2
876 {
877 uint8_t UARTTransmitData[TX_PACKET_LENGTH]; //Data buffer
878
879 PORTA |= LED_OUT; //Turn on LED_OUT (PORTA.2) to symbolize outgoing packet
880
881 UARTTransmitData[0] = eeprom_read_byte((uint8_t*)&ConfigStructEEPROM.MaxAcc); //max. acceleration
882 UARTTransmitData[1] = eeprom_read_byte((uint8_t*)&ConfigStructEEPROM.Kp); //Kp
883 UARTTransmitData[2] = eeprom_read_byte((uint8_t*)&ConfigStructEEPROM.Kd); //Kd
884 UARTTransmitData[3] = eeprom_read_byte((uint8_t*)&ConfigStructEEPROM.Ki); //Ki
885
886 UARTTransmitData[4] = (int8_t)ISRsSharedStruct.ActualRPMR; //actual right wheel speed (imp/50ms
887 UARTTransmitData[5] = (int8_t)ISRsSharedStruct.ActualRPML; //actual left wheel speed (imp/50ms)
888
889 UARTTransmitData[6] = (uint8_t)(ISRsSharedStruct.TotalImpulsesR>>8); //buffer high byte
890 UARTTransmitData[7] = (uint8_t)(ISRsSharedStruct.TotalImpulsesR & 0x00FF); //buffer low byte
891
892 UARTTransmitData[8] = (uint8_t)(ISRsSharedStruct.TotalImpulsesL>>8); //buffer high byte
893 UARTTransmitData[9] = (uint8_t)(ISRsSharedStruct.TotalImpulsesL & 0x00FF); //buffer low byte
894
895 UARTTransmitData[10] = eeprom_read_byte((uint8_t*)&ConfigStructEEPROM.RegErrorFilter); //reg. error threshold
896
897 UARTTransmitData[TX_CHECKSUM_BYTE] = 0; //reset checksum
898
899 for(uint8_t i = 0; i < TX_CHECKSUM_BYTE; i++)
900 {
901 UARTTransmitData[TX_CHECKSUM_BYTE] += UARTTransmitData[i]; //calculate checksum
902
903 UART_SendByte((uint8_t)UARTTransmitData[i]); //send data via UART
904 }
905
906 UART_SendByte((uint8_t)UARTTransmitData[TX_CHECKSUM_BYTE]); //send checksum via UART
907
908 break;
909
910 }
911
912 case SEND_SRF08: //SRF08 & position regulator info
913 {
914 uint8_t UARTTransmitData[TX_PACKET_LENGTH]; //Data buffer
915
916 PORTA |= LED_OUT; //Turn on LED_OUT (PORTA.2) to symbolize outgoing packet
917
918 UARTTransmitData[0] = eeprom_read_byte((uint8_t*)&ConfigStructEEPROM.Omega0); //omega0
919 UARTTransmitData[1] = eeprom_read_byte((uint8_t*)&ConfigStructEEPROM.L); //distance between sensors
920 UARTTransmitData[2] = eeprom_read_byte((uint8_t*)&ConfigStructEEPROM.Ksi); //Ksi
921
922 UARTTransmitData[3] = (uint8_t)(ISRsSharedStruct.Ky>>8); //high byte of Ky
923 UARTTransmitData[4] = (uint8_t)(ISRsSharedStruct.Ky & 0x00FF); //low byte of Ky
924
925 UARTTransmitData[5] = (uint8_t)(ISRsSharedStruct.Kfi>>8); //high byte of Kfi
926 UARTTransmitData[6] = (uint8_t)(ISRsSharedStruct.Kfi & 0x00FF); //low byte of Kfi
927
928 UARTTransmitData[7] = (uint8_t)ISRsSharedStruct.V_des; //desired speed
929 UARTTransmitData[8] = (uint8_t)ISRsSharedStruct.y0_des; //desired distance from the wall
930
931 UARTTransmitData[9] = (uint8_t)ISRsSharedStruct.SRF08_DistanceFront; //send front distance measured by SRF08
932 UARTTransmitData[10] =(uint8_t)ISRsSharedStruct.SRF08_DistanceRear; //send rear distance measured by SRF08
933
934 UARTTransmitData[TX_CHECKSUM_BYTE] = 0; //reset checksum
935
936 for(uint8_t i = 0; i < TX_CHECKSUM_BYTE; i++)
937 {
938 UARTTransmitData[TX_CHECKSUM_BYTE] += UARTTransmitData[i]; //calculate checksum
939
940 UART_SendByte((uint8_t)UARTTransmitData[i]); //send data via UART
941 }
942
943 UART_SendByte((uint8_t)UARTTransmitData[TX_CHECKSUM_BYTE]); //send checksum via UART
944
945 break;
946 }
947
948 case SEND_REGULATORS: //Position regulators info
949 {
950 uint8_t UARTTransmitData[TX_PACKET_LENGTH]; //Data buffer
951
952 PORTA |= LED_OUT; //Turn on LED_OUT (PORTA.2) to symbolize outgoing packet
953
954 UARTTransmitData[0] = (uint8_t)(ISRsSharedStruct.Xdes>>8); //Xdes HB
955 UARTTransmitData[1] = (uint8_t)(ISRsSharedStruct.Xdes & 0x00FF); //Xdes LB
956
957 UARTTransmitData[2] = (uint8_t)(ISRsSharedStruct.Ydes>>8); //Ydes HB
958 UARTTransmitData[3] = (uint8_t)(ISRsSharedStruct.Ydes & 0x00FF); //Ydes LB
959
960 UARTTransmitData[4] = eeprom_read_byte((uint8_t*)&ConfigStructEEPROM.RegGamma); //Gamma
961 UARTTransmitData[5] = eeprom_read_byte((uint8_t*)&ConfigStructEEPROM.RegEpsilon);//Epsilon
962 UARTTransmitData[6] = eeprom_read_byte((uint8_t*)&ConfigStructEEPROM.RegAngleGain); //Angle feedback loop gain
963 UARTTransmitData[7] = eeprom_read_byte((uint8_t*)&ConfigStructEEPROM.RegPositionGain); //Position feedback gain
964 UARTTransmitData[8] = eeprom_read_byte((uint8_t*)&ConfigStructEEPROM.RegSpeed); //Regulator constant speed
965
966 UARTTransmitData[9] = 0; //Empty
967 UARTTransmitData[10] = 0;
968
969 UARTTransmitData[TX_CHECKSUM_BYTE] = 0; //reset checksum
970
971 for(uint8_t i = 0; i < TX_CHECKSUM_BYTE; i++)
972 {
973 UARTTransmitData[TX_CHECKSUM_BYTE] += UARTTransmitData[i]; //calculate checksum
974
975 UART_SendByte((uint8_t)UARTTransmitData[i]); //send data via UART
976 }
977
978 UART_SendByte((uint8_t)UARTTransmitData[TX_CHECKSUM_BYTE]); //send checksum via UART
979
980 break;
981 }
982 }
983 }
984
985 ISRsSharedStruct.UARTRxIndex = 0; //Reset the Rx index
986
987 PORTA &= ~( LED_IN | LED_OUT ); //Turn off LED_IN (PORTA.1) & LED_OUT (PORTA.2)
988 }
989 }
990
995 ISR(TIMER0_OVF_vect)
996 {
997 if(++ISRsSharedStruct.Timer0Prescaler >= 4) //every 4 ms
998 {
999 ISRsSharedStruct.UARTRxIndex = 0; //reset RXindex, period between two bytes received via UART must be shorter than 4 ms, otherwise it is considered to be a new packet
1000 ISRsSharedStruct.Timer0Prescaler = 0; //reset prescaler
1001 }
1002 }
1003
1008 ISR(TIMER2_OVF_vect)
1009 {
1010 static uint8_t Timer2prescaler = 0; //Counter of software prescaling
1011 static int16_t RegErrorR_Prev = 0, RegErrorL_Prev = 0; //Previous error
1012 static uint8_t RegOutputR_Prev = SPEED_OFFSET, RegOutputL_Prev = SPEED_OFFSET; //Previous regulator outputs to calculate acceleration ramp (must add the offset (128)!)
1013 static uint8_t DistanceSafetySensorFirst = 0, DistanceSafetySensorSecond = 0; //Distances mesured by safety sensors
1014
1015 switch(Timer2prescaler)
1016 {
1017 default: //Increase the prescaler
1018 Timer2prescaler++;
1019
1020 break;
1021
1022
1023 case TIMER2_SEND_SRF08_COMMAND: //Generate write command (measurement in centimeters), check UART watchdog
1024 Timer2prescaler++;
1025
1026 //SRF08 start measurement sequence
1027 SRF08_Write(ConfigStructRAM.SRF08_Front,0x00, 0x51); //front SRF08 command to start meas.
1028 SRF08_Write(ConfigStructRAM.SRF08_Rear,0x00, 0x51); //rear SRF08 command to start meas.
1029
1030 //Optionally start measuring using first safety sensor
1031 if(ConfigStructRAM.SRF08_SafetyFirst >= SRF08_LOWEST_ADDR && ConfigStructRAM.SRF08_SafetyFirst <= SRF08_HIGHEST_ADDR)
1032 {
1033 SRF08_Write(ConfigStructRAM.SRF08_SafetyFirst,0x00, 0x51); //rear SRF08 command to start meas.
1034 }
1035
1036 //Optionally start measuring using second safety sensor
1037 if(ConfigStructRAM.SRF08_SafetySecond >= SRF08_LOWEST_ADDR && ConfigStructRAM.SRF08_SafetySecond <= SRF08_HIGHEST_ADDR)
1038 {
1039 SRF08_Write(ConfigStructRAM.SRF08_SafetySecond,0x00, 0x51); //rear SRF08 command to start meas.
1040 }
1041
1042 //Check UART watchdog status
1043 if(++ISRsSharedStruct.UARTWatchdog >= UART_WATCHDOG_TIMEOUT) //no data received for specified interval -> stop motors
1044 {
1045 ISRsSharedStruct.EnableWallFollowing = FALSE; //disable wall following algorithm
1046
1047 ISRsSharedStruct.EnablePositionRegulator = FALSE; //disable position regulator
1048
1049 ISRsSharedStruct.EnableSpRegR = FALSE; //disable speed regulator
1050 ISRsSharedStruct.EnableSpRegL = FALSE; //disable speed regulator
1051
1052 ISRsSharedStruct.RegIntegratorR = 0; //reset the integrator
1053 ISRsSharedStruct.RegIntegratorL = 0; //reset the integrator
1054
1055 ISRsSharedStruct.MomSpeedR = SPEED_OFFSET; //zero speed
1056 ISRsSharedStruct.MomSpeedL = SPEED_OFFSET; //zero speed
1057
1058 }
1059
1060 break;
1061
1062 case TIMER2_READ_SRF08_DATA: //Read data from SRF08 sensors
1063 Timer2prescaler++;
1064
1065 ISRsSharedStruct.SRF08_DistanceFront = SRF08_Read(ConfigStructRAM.SRF08_Front,0x03); //read data from front sensor
1066 ISRsSharedStruct.SRF08_DistanceRear = SRF08_Read(ConfigStructRAM.SRF08_Rear,0x03); //read data from rear sensor
1067
1068 //Optionally start measuring using first safety sensor
1069 if(ConfigStructRAM.SRF08_SafetyFirst >= SRF08_LOWEST_ADDR && ConfigStructRAM.SRF08_SafetyFirst <= SRF08_HIGHEST_ADDR)
1070 {
1071 DistanceSafetySensorFirst = SRF08_Read(ConfigStructRAM.SRF08_SafetyFirst,0x03);
1072 }
1073 else
1074 {
1075 DistanceSafetySensorFirst = 0;
1076 }
1077
1078 //Optionally start measuring using second safety sensor
1079 if(ConfigStructRAM.SRF08_SafetySecond >= SRF08_LOWEST_ADDR && ConfigStructRAM.SRF08_SafetySecond <= SRF08_HIGHEST_ADDR)
1080 {
1081 DistanceSafetySensorSecond = SRF08_Read(ConfigStructRAM.SRF08_SafetySecond,0x03);
1082 }
1083 else
1084 {
1085 DistanceSafetySensorSecond = 0;
1086 }
1087
1088 break;
1089
1090 case TIMER2_SPEED_REGULATORS: //PSD speed regulators
1091 {
1092 int16_t RegOutputR, RegOutputL; //output signal of PSD regulators
1093 int16_t RegErrorR, RegErrorL; //errors (desired - actual values)
1094
1095 Timer2prescaler = 0;
1096
1097 //Safety feedback (front sensor) - if distance measured by front SRF08 is lower than specified, stop motors
1098 if((ISRsSharedStruct.SRF08_DistanceFront <= SRF08_SAFETY_RANGE) && (ISRsSharedStruct.SRF08_DistanceFront))
1099 {
1100 //Stop motors immediately
1101 OCR1A = (unsigned int)(SPEED_OFFSET + UCHAR_TO_RC_SIGNAL);
1102 OCR1B = (unsigned int)(SPEED_OFFSET + UCHAR_TO_RC_SIGNAL);
1103
1104 break;
1105 }
1106
1107 //Safety feedback (rear sensor)
1108 if((ISRsSharedStruct.SRF08_DistanceRear <= SRF08_SAFETY_RANGE) && (ISRsSharedStruct.SRF08_DistanceRear))
1109 {
1110 //Stop motors immediately
1111 OCR1A = (unsigned int)(SPEED_OFFSET + UCHAR_TO_RC_SIGNAL);
1112 OCR1B = (unsigned int)(SPEED_OFFSET + UCHAR_TO_RC_SIGNAL);
1113
1114 break;
1115 }
1116
1117 //Safety feedback (first safety sensor)
1118 if((DistanceSafetySensorFirst <= SRF08_SAFETY_RANGE) && DistanceSafetySensorFirst)
1119 {
1120 //Stop motors immediately
1121 OCR1A = (unsigned int)(SPEED_OFFSET + UCHAR_TO_RC_SIGNAL);
1122 OCR1B = (unsigned int)(SPEED_OFFSET + UCHAR_TO_RC_SIGNAL);
1123
1124 break;
1125 }
1126
1127 //Safety feedback (second safety sensor)
1128 if((DistanceSafetySensorSecond <= SRF08_SAFETY_RANGE) && DistanceSafetySensorSecond)
1129 {
1130 //Stop motors immediately
1131 OCR1A = (unsigned int)(SPEED_OFFSET + UCHAR_TO_RC_SIGNAL);
1132 OCR1B = (unsigned int)(SPEED_OFFSET + UCHAR_TO_RC_SIGNAL);
1133
1134 break;
1135 }
1136
1137
1138 ISRsSharedStruct.ActualRPMR = ISRsSharedStruct.BufferSpeedR; //impulses per 50 ms
1139 ISRsSharedStruct.ActualRPML = ISRsSharedStruct.BufferSpeedL; //impulses per 50 ms
1140
1141 ISRsSharedStruct.BufferSpeedR = 0; //reset the buffer
1142 ISRsSharedStruct.BufferSpeedL = 0; //reset the buffer
1143
1144 if(ISRsSharedStruct.EnableSpRegR) //if right motor regulator is enabled RegOutput must be calculated, else it is equal to desired momentum
1145 {
1146 RegErrorR = (int16_t)((ISRsSharedStruct.MomSpeedR - SPEED_OFFSET) - ISRsSharedStruct.ActualRPMR); //calculate error (substract the offset!)
1147
1148 if((abs(RegErrorR) <= ConfigStructRAM.RegErrorFilter) && ISRsSharedStruct.MomSpeedR != SPEED_OFFSET) //filter the error (disable if too close to zero speed)
1149 {
1150 RegErrorR = 0;
1151 }
1152
1153 ISRsSharedStruct.RegIntegratorR += RegErrorR; //integrate error
1154
1155 if(ISRsSharedStruct.RegIntegratorR > INTEGRATOR_MAX) //prevent integrator overflow
1156 {
1157 ISRsSharedStruct.RegIntegratorR = INTEGRATOR_MAX;
1158 }
1159
1160 if(ISRsSharedStruct.RegIntegratorR < INTEGRATOR_MIN) //prevent integrator underflow
1161 {
1162 ISRsSharedStruct.RegIntegratorR = INTEGRATOR_MIN;
1163 }
1164
1165 RegOutputR = (int16_t)(((ConfigStructRAM.Kp * RegErrorR) + (ConfigStructRAM.Ki * ISRsSharedStruct.RegIntegratorR) + (ConfigStructRAM.Kd * (RegErrorR - RegErrorR_Prev)))/10); //calculate regulator output (must be divided by 10, constants are multiplied by 10!)
1166
1167 RegErrorR_Prev = RegErrorR; //save previous error
1168
1169 RegOutputR += SPEED_OFFSET; //add the offset (128 = zero speed)
1170 }
1171 else
1172 {
1173 RegOutputR = (int16_t)ISRsSharedStruct.MomSpeedR;
1174 }
1175
1176 if(ISRsSharedStruct.EnableSpRegL) //if left motor regulator is enabled RegOutput must be calculated, else it is equal to desired momentum
1177 {
1178 RegErrorL = (int16_t)((ISRsSharedStruct.MomSpeedL - SPEED_OFFSET) - ISRsSharedStruct.ActualRPML); //calculate error (substract the offset!)
1179
1180 if((abs(RegErrorL) <= ConfigStructRAM.RegErrorFilter) && ISRsSharedStruct.MomSpeedL != SPEED_OFFSET) //filter the error (disable if too close to zero speed)
1181 {
1182 RegErrorL = 0;
1183 }
1184
1185 ISRsSharedStruct.RegIntegratorL += RegErrorL; //integrate error
1186
1187 if(ISRsSharedStruct.RegIntegratorL > INTEGRATOR_MAX) //prevent integrator overflow
1188 {
1189 ISRsSharedStruct.RegIntegratorL = INTEGRATOR_MAX;
1190 }
1191
1192 if(ISRsSharedStruct.RegIntegratorL < INTEGRATOR_MIN) //prevent integrator underflow
1193 {
1194 ISRsSharedStruct.RegIntegratorL = INTEGRATOR_MIN;
1195 }
1196
1197 RegOutputL = (int16_t)(((ConfigStructRAM.Kp * RegErrorL) + (ConfigStructRAM.Ki * ISRsSharedStruct.RegIntegratorL) + (ConfigStructRAM.Kd * (RegErrorL - RegErrorL_Prev)))/10); //calculate regulator output (must be divided by 10, constants are multiplied by 10!)
1198
1199 RegErrorL_Prev = RegErrorL; //save previous error
1200
1201 RegOutputL += SPEED_OFFSET; //add the offset (128 = zero speed)
1202 }
1203 else
1204 {
1205 RegOutputL = (int16_t)ISRsSharedStruct.MomSpeedL;
1206 }
1207
1208 if (abs(RegOutputR - RegOutputR_Prev) > ConfigStructRAM.MaxAcc) //right motor acceleration ramp
1209 {
1210 if(RegOutputR > RegOutputR_Prev) //difference cannot be greater than MaxAcc value
1211 {
1212 RegOutputR = RegOutputR_Prev + ConfigStructRAM.MaxAcc;
1213 }
1214 else
1215 {
1216 RegOutputR = RegOutputR_Prev - ConfigStructRAM.MaxAcc;
1217 }
1218 }
1219
1220 if (abs(RegOutputL - RegOutputL_Prev) > ConfigStructRAM.MaxAcc) //left motor acceleration ramp
1221 {
1222 if(RegOutputL > RegOutputL_Prev) //difference cannot be greater than MaxAcc value
1223 {
1224 RegOutputL = RegOutputL_Prev + ConfigStructRAM.MaxAcc;
1225 }
1226 else
1227 {
1228 RegOutputL = RegOutputL_Prev - ConfigStructRAM.MaxAcc;
1229 }
1230 }
1231
1232 if(RegOutputR > REG_OUTPUT_MAX) //right motor saturation & anti-windup
1233 {
1234 //ISRsSharedStruct.RegIntegratorR -= (int16_t)(RegOutputR - REG_OUTPUT_MAX);
1235
1236 RegOutputR = REG_OUTPUT_MAX;
1237 }
1238
1239 if(RegOutputR < REG_OUTPUT_MIN) //right motor saturation & anti-windup
1240 {
1241 //ISRsSharedStruct.RegIntegratorR -= RegOutputR;
1242
1243 RegOutputR = REG_OUTPUT_MIN;
1244 }
1245
1246
1247 if(RegOutputL > REG_OUTPUT_MAX) //left motor saturation & anti-windup
1248 {
1249 //ISRsSharedStruct.RegIntegratorL -= (int16_t)(RegOutputL - REG_OUTPUT_MAX);
1250
1251 RegOutputL = REG_OUTPUT_MAX;
1252 }
1253
1254 if(RegOutputL < REG_OUTPUT_MIN) //left motor saturation & anti-windup
1255 {
1256 //ISRsSharedStruct.RegIntegratorL -= RegOutputL;
1257
1258 RegOutputL = REG_OUTPUT_MIN;
1259 }
1260
1261 RegOutputR_Prev = RegOutputR; //save previous reg. output
1262 RegOutputL_Prev = RegOutputL; //save previous reg. output
1263
1264 OCR1A = (unsigned int)(RegOutputL + UCHAR_TO_RC_SIGNAL); //calculate values of OCR1A(left wheel) signal generator registers
1265 OCR1B = (unsigned int)(RegOutputR + UCHAR_TO_RC_SIGNAL); //calculate values of OCR1B (right wheel) signal generator registers
1266
1267 break;
1268
1269 }
1270 }
1271
1272 WATCHDOG_RESET(); //watchdog reset
1273 }
1274
1278 ISR(INT0_vect)
1279 {
1280 if(LEFT_WHEEL_STEP) //Rising edge
1281 {
1282 if(LEFT_WHEEL_DIR) //90 degrees phase shifted signal on pin LEFT_WHEEL_DIR determines the direction
1283 {
1284 ISRsSharedStruct.BufferOdoL--;
1285 ISRsSharedStruct.BufferSpeedL--;
1286 ISRsSharedStruct.TotalImpulsesL--;
1287 }
1288 else
1289 {
1290 ISRsSharedStruct.BufferOdoL++;
1291 ISRsSharedStruct.BufferSpeedL++;
1292 ISRsSharedStruct.TotalImpulsesL++;
1293 }
1294 }
1295 else //Falling edge
1296 {
1297 if(LEFT_WHEEL_DIR) //90 degrees phase shifted signal on pin LEFT_WHEEL_DIR determines the direction
1298 {
1299 ISRsSharedStruct.BufferOdoL++;
1300 ISRsSharedStruct.BufferSpeedL++;
1301 ISRsSharedStruct.TotalImpulsesL++;
1302 }
1303 else
1304 {
1305 ISRsSharedStruct.BufferOdoL--;
1306 ISRsSharedStruct.BufferSpeedL--;
1307 ISRsSharedStruct.TotalImpulsesL--;
1308 }
1309 }
1310
1311 if(GIFR & (1 << INTF0)) //debounce - if INT0 flag has been set, clear it (is cleared when 1 is written)
1312 {
1313 GIFR &= (1 << INTF0);
1314 }
1315 }
1316
1320 ISR(INT1_vect)
1321 {
1322 if(RIGHT_WHEEL_STEP) //Rising edge
1323 {
1324 if(RIGHT_WHEEL_DIR) //90 degrees phase shifted signal on pin RIGHT_WHEEL_DIR determines the direction
1325 {
1326 ISRsSharedStruct.BufferOdoR++;
1327 ISRsSharedStruct.BufferSpeedR++;
1328 ISRsSharedStruct.TotalImpulsesR++;
1329 }
1330 else
1331 {
1332 ISRsSharedStruct.BufferOdoR--;
1333 ISRsSharedStruct.BufferSpeedR--;
1334 ISRsSharedStruct.TotalImpulsesR--;
1335 }
1336 }
1337 else //Falling edge
1338 {
1339 if(RIGHT_WHEEL_DIR) //90 degrees phase shifted signal on pin RIGHT_WHEEL_DIR determines the direction
1340 {
1341 ISRsSharedStruct.BufferOdoR--;
1342 ISRsSharedStruct.BufferSpeedR--;
1343 ISRsSharedStruct.TotalImpulsesR--;
1344 }
1345 else
1346 {
1347 ISRsSharedStruct.BufferOdoR++;
1348 ISRsSharedStruct.BufferSpeedR++;
1349 ISRsSharedStruct.TotalImpulsesR++;
1350 }
1351 }
1352
1353 if(GIFR & (1 << INTF1)) //debounce - if INT1 flag has been set, clear it (is cleared when 1 is written)
1354 {
1355 GIFR &= (1 << INTF1);
1356 }
1357 }
1358
1364 ISR(INT2_vect)
1365 {
1366 GLOBAL_INTERRUPTS_DISABLE(); //disable all interrupts
1367
1368 PORTC &= ~(0xFC); //turn all power outputs off (pins 0/1 are used as SCL/SDA of I2C interface)
1369
1370 OCR1A = SPEED_OFFSET + UCHAR_TO_RC_SIGNAL; //left motor stop
1371 OCR1B = SPEED_OFFSET + UCHAR_TO_RC_SIGNAL; //right motor stop
1372
1373 TIMER_1_OFF(); //TIMER1 off - turn off R/C signal
1374
1375 while(TRUE) //infinite loop (blink the LEDs)
1376 {
1377 BLINK_LEDS_MS(100); //Blink LEDs
1378
1379 WATCHDOG_RESET(); //Reset watchdog
1380 }
1381 }