1 //DeadReckoning
  2 //This function handles the motion awareness, based on encoder readings
  3 
  4 //#define TEST
  5 //#define TIMER_TEST
  6 //#define Wheel_Speed_TEST
  7 
  8 #include <math.h>
  9 
 10 
 11 #include <mc9s12e128.h>     /* derivative information */
 12 #include <S12E128bits.h> /* E128 bit definitions  */
 13 #include <stdio.h>
 14 #include <stdlib.h>
 15 
 16 #include <hidef.h>      /* common defines and macros */
 17 #include <ADS12.h>
 18 #include <bitdefs.h> /* BIT0HI, BIT0LO....definitions */
 19 
 20 
 21 #include "S12eVec.h"
 22 #include "DeadReckoning.h"
 23 #include "Movement.h"
 24 #include "InitPorts.h"
 25 #include "PWM.h"
 26 #include "Timers.h"
 27 #include "Constants.h"
 28 #include "Sensors.h"
 29 
 30 
 31 #define _Kp_R    1.5
 32 #define _Ki_R     .4
 33 #define _Kp_L    1.5
 34 #define _Ki_L     .4
 35 #define UPDATE_TIMER 187  //Updates at ~1kHz
 36 
 37 static float Xpos=0, Ypos=0;      //Measures in inches
 38 static int Angle=0, LeftTicks, RightTicks;  //Measures in ticks
 39 static unsigned int LeftEncodePeriod, RightEncodePeriod;
 40 
 41 static int LEFT_SPEED=0, RIGHT_SPEED=0;
 42 
 43 static unsigned int j = 0;
 44 static unsigned int k = 0;
 45 float RCurrentSpeed, LCurrentSpeed, LInt, RInt, LError, RError;
 46 char LDutyCycle, RDutyCycle;
 47 
 48 //////////////////////////////////////////////////////////////
 49 //Left Wheel Encoder Read
 50 void interrupt _Vec_tim1ch6 LeftEncode(void){
 51   //static int i_left=0;
 52   static unsigned int LastEdge;
 53   unsigned int NewEdge;
 54 
 55   NewEdge = TIM1_TC6;
 56   LeftEncodePeriod = NewEdge - LastEdge;
 57 
 58   //(void)printf("NewEdge: %u, LastEdge: %u, Period: %u \r\n", NewEdge, LastEdge, EncodePeriod );
 59   LastEdge = NewEdge;
 60 
 61 
 62   //Wheel Direction is inferred by direction motor is being driven
 63   //(our bot should never apply power in opposite direction)
 64   //and encoder readings are potentially comprised in event of collision anywars
 65 
 66   if(LeftForward){  //Moving forward
 67     //Mesaured in ticks (# of ticks / TicksPerRotation = 1 rotation)
 68     LeftTicks++;
 69   }
 70   else{
 71     LeftTicks--;
 72   }
 73 
 74   TIM1_TFLG1 = _S12_C6F;   //Clear the flag
 75 
 76 }
 77 
 78 
 79 ///////////////////////////////////////////////////////////////
 80 //Right Wheel Encoder Read
 81 void interrupt _Vec_tim1ch5 RightEncode(void){
 82 
 83   //static int i_right=0;
 84   static unsigned int LastEdge;
 85   unsigned int NewEdge;
 86 
 87 
 88   NewEdge = TIM1_TC5;
 89   RightEncodePeriod = NewEdge - LastEdge;
 90 
 91   LastEdge = NewEdge;
 92 
 93 
 94   if(RightForward){  //Moving forward
 95     //Mesaured in ticks (# of ticks / TicksPerRotation = 1 rotation)
 96     RightTicks++;
 97   }
 98   else{
 99     RightTicks--;
100   }
101 
102   TIM1_TFLG1 = _S12_C5F;   //Clear the flag
103 }
104 
105 
106 
107 
108 
109 
110 /////////////////////////////////////////////////////////////////
111 //PID on the wheels
112 
113 //Performs internal positioning calculations for both left and right wheels
114 void interrupt _Vec_tim0ch4 Wheel_PID(void)
115 {
116 
117   static float LeftIntegral=0, RightIntegral=0;
118   static float Last_RightTarget, Last_LeftTarget;
119 
120   float RightTarget_in, LeftTarget_in;
121   float CurrentSpeed, Error;
122   int Period;
123   float DutyCycle;
124 
125     TIM0_TC4 = TIM0_TCNT + UPDATE_TIMER;  //Reset Timer
126     TIM0_TFLG1 = _S12_C4F;   //Clear the flag
127     EnableInterrupts;   //We don't want to miss encoder ticks
128 
129 
130 
131 
132 
133   //Scale and convert target speeds
134   LeftTarget_in = LEFT_SPEED/100.0;
135   RightTarget_in = RIGHT_SPEED/100.0;
136 
137   if( RightTarget_in == 0)
138   {
139     RightIntegral = 0;
140   }
141 
142   if( LeftTarget_in == 0)
143   {
144     LeftIntegral = 0;
145   }
146 
147 
148   Last_RightTarget = RightTarget_in;
149   Last_LeftTarget = LeftTarget_in;
150 
151   //////////////////////////////////////////////////
152   //Start Calculations for the left side
153   //////////////////////////////////////////////////
154     Period = LeftEncodePeriod;
155     //if(Period < 50)
156     //  return;
157 
158     //35.28 converts speed to roughly Rotations per second
159     //(precise units unneccessary as long as they are internally consistent)
160     CurrentSpeed = 35.28/(float)Period;
161     LCurrentSpeed =  CurrentSpeed;
162 
163     if (LeftTarget_in < 0)
164     {
165       LeftTarget_in = -LeftTarget_in;
166       Error = LeftTarget_in - CurrentSpeed;
167       LError = Error;
168       LeftIntegral += Error;
169       LInt = LeftIntegral;
170       DutyCycle = (Error * _Kp_L + LeftIntegral * _Ki_L);
171 
172       if(DutyCycle > 100)
173       {
174         DutyCycle = 100;  //Duty cycle can't be more than max value
175         LeftIntegral -= Error;  //Unwind the overshoot
176 
177         //Alerts use that threshold has been reached and physical limitations prevent proper operation
178         //(void)printf("MAX HIT!!!!!!!!!!!!!!!!! MAX HIT!!!!!!!!!!!!!!!!!!");
179       }
180 
181       if( DutyCycle < 0 )
182       {
183         DutyCycle= 0;  //Duty cycle can't be more than max value
184         LeftIntegral -= Error;//Unwind the overshoot
185       }
186 
187       LDutyCycle = (char)DutyCycle;
188       LeftBack((char)DutyCycle);
189     }
190     else
191     {
192       Error = LeftTarget_in - CurrentSpeed;
193       LError = Error;
194       LeftIntegral += Error;
195       LInt = LeftIntegral;
196       DutyCycle = (Error * _Kp_L + LeftIntegral * _Ki_L);
197 
198 
199       if(DutyCycle > 100)
200       {
201         DutyCycle = 100;  //Duty cycle can't be more than max value
202         LeftIntegral -= Error;  //Unwind the overshoot
203 
204         //Alerts use that threshold has been reached and physical limitations prevent proper operation
205         //(void)printf("MAX HIT!!!!!!!!!!!!!!!!! MAX HIT!!!!!!!!!!!!!!!!!!");
206       }
207 
208       if( DutyCycle < 0 )
209       {
210         DutyCycle= 0;  //Duty cycle can't be more than max value
211         LeftIntegral -= Error;//Unwind the overshoot
212       }
213 
214       LDutyCycle = (char)DutyCycle;
215       LeftFor((char)DutyCycle);
216     }
217 
218   //////////////////////////////////////////////////
219   //Start Calculations for the right side
220   //////////////////////////////////////////////////
221     Period = RightEncodePeriod;
222 
223     //if(Period < 50)
224     //  return;
225 
226     // 1000ms/s * ( (187tick/ms) / (12encode/rotation) ) / (ticks/encode)
227     // = rotations/second
228     //1000*187/12 = 15583.3
229     CurrentSpeed = 35.28/(float)Period;
230     RCurrentSpeed =  CurrentSpeed;
231 
232     if (RightTarget_in < 0)
233     {
234       RightTarget_in = -RightTarget_in;
235       Error = RightTarget_in - CurrentSpeed;
236       RError = Error;
237       RightIntegral += Error;
238       RInt = RightIntegral;
239       DutyCycle = (Error * _Kp_R + RightIntegral * _Ki_R);
240 
241       if(DutyCycle > 100)
242       {
243         DutyCycle = 100;  //Duty cycle can't be more than max value
244         RightIntegral -= Error;  //Unwind the overshoot
245       }
246 
247       if(DutyCycle < 0)
248       {
249         DutyCycle= 0;  //Duty cycle can't be more than max value
250         RightIntegral -= Error;//Unwind the overshoot
251       }
252 
253       RDutyCycle = (char)(DutyCycle);
254       RightBack((char)(DutyCycle));
255     }
256     else
257     {
258       Error = RightTarget_in - CurrentSpeed;
259       RError = Error;
260       RightIntegral += Error;
261       RInt = RightIntegral;
262       DutyCycle = (Error * _Kp_R + RightIntegral * _Ki_R);
263 
264       if(DutyCycle > 100)
265       {
266         DutyCycle = 100;  //Duty cycle can't be more than max value
267         RightIntegral -= Error;  //Unwind the overshoot
268       }
269 
270       if(DutyCycle < 0)
271       {
272         DutyCycle= 0;  //Duty cycle can't be more than max value
273         RightIntegral -= Error;//Unwind the overshoot
274       }
275 
276       RDutyCycle = (char)(DutyCycle);
277       RightFor((char)(DutyCycle));
278     }
279 
280 }
281 
282 
283 
284 
285 
286 
287 
288 
289 ////////////////////////////////////////////////////////////////
290 //Calculates Distance Change
291 //void interrupt _Vec_tim0ch5 PositionControl(void){
292 //The event needs to process very quickly for accuracy (make it high on the priority list)
293 void PositionMath(void)
294 {
295   static int OldLeftTicks, OldRightTicks;
296   int NewLeftTicks, NewRightTicks, DeltaTicks, DeltaLeft, DeltaRight;
297 
298   if((TIM0_TFLG1 & _S12_C5F) != 0){
299     //(void)printf("inside if statement");
300     TIM0_TC5 = TIM0_TCNT + UPDATE_TIMER;   //Update the timer
301     TIM0_TFLG1 = _S12_C5F;   //Clear the flag
302 
303     NewLeftTicks = LeftTicks;
304     NewRightTicks = RightTicks;
305 
306     //Error due to small angle approx ~ (ticks/10000)^2
307     //for ticks = 1, sin(ticks)/ticks ~ 10^-8
308     //for ticks = 100,  sin(ticks)/ticks ~ 10^-4
309     DeltaLeft = (NewLeftTicks-OldLeftTicks);
310     DeltaRight = (NewRightTicks - OldRightTicks);
311     DeltaTicks = DeltaRight + DeltaLeft;
312 
313     Angle += DeltaRight - DeltaLeft;  //This is probably correct
314     //Angle += DeltaLeft - DeltaRight;
315     /*if(Angle > TicksPerRotation/2)
316     {
317       Angle = Angle - TicksPerRotation;
318     }
319     if(Angle < -TicksPerRotation/2)
320     {
321       Angle = Angle + TicksPerRotation;
322     }
323     printf ("Angle is %i\r\n", Angle);
324     */
325 
326     //Divide by 2 accounts for 2 wheels moving in same direction
327     Xpos += DeltaTicks * DistancePerTick * cos(Angle*(2*PI/TicksPerRotation)) / 2;
328     Ypos += DeltaTicks * DistancePerTick * sin(Angle*(2*PI/TicksPerRotation)) / 2;
329 
330 
331     //(void)printf("NewLeft: %i  OldLeft: %i  NewRight: %i  OldRight: %i\n\r", NewLeftTicks, OldLeftTicks, NewRightTicks, OldRightTicks);
332     //(void)printf("Angle: %6.5f, X-position: %6.3f, Y-position: %6.3f\n\r", (Angle*(2*PI/TicksPerRotation)),Xpos,Ypos);
333 
334     OldLeftTicks = NewLeftTicks;
335     OldRightTicks = NewRightTicks;
336 
337   }
338 }
339 
340 
341 ///////////////////////////////////////////////////////////////
342 //External Data calls: since I have the interrupts in this file,
343 //these can be used to access the variables instead of module lvl vars
344 //Interrupts disable around the variable call to avoid manipulating 'half of a float'
345 
346 int GetAngle(void){
347   int ReturnAngle;
348   //DisableInterrupts;
349   ReturnAngle = Angle;
350   //EnableInterrupts;
351  return ReturnAngle;
352 }
353 
354 void ReckonAngle(int NewAngle){
355   Angle = NewAngle;
356 }
357 
358 float GetXpos(void){
359   float ReturnXpos;
360   ReturnXpos = Xpos;
361  return ReturnXpos;
362 }
363 
364 
365 
366 float GetYpos(void){
367   float ReturnYpos;
368   ReturnYpos = Ypos;
369  return ReturnYpos;
370 }
371 
372 void ReckonXpos(float X){
373   Xpos = X;
374 }
375 
376 void ReckonYpos(float Y){
377   Ypos = Y;
378 }
379 
380 
381 
382 ////////////////////////////////////////////////////////////////////////
383 //Test Harnesses
384 
385 
386 #ifdef TIMER_TEST
387 void main(void)
388 {
389   float LeftTarget = 0, RightTarget = 0;
390   char KeyStroke;
391 
392   (void)printf("Starting");
393   InitPorts();
394   InitPWM();
395   InitTimers();
396 
397   while(1)
398   {
399 
400     if ( kbhit() != 0)
401     {
402       KeyStroke = getchar();
403 
404       switch (KeyStroke)
405       {
406 
407         case '8':
408           LeftTarget+=.1;
409           RightTarget+=.1;
410           break;
411         case '2':
412           LeftTarget-=.1;
413           RightTarget-=.1;
414           break;
415         case '7':
416           LeftTarget+=.1;
417           break;
418         case '1':
419           LeftTarget-=.1;
420           break;
421         case '9':
422           RightTarget+=.1;
423           break;
424         case '3':
425           RightTarget-=.1;
426           break;
427       }
428       //(void)printf("RightTarget: %f    LeftTarget:  %f", RightTarget, LeftTarget);
429     }
430 
431     PositionMath();
432     //WheelSpeed(LeftTarget, RightTarget);
433     LeftSpeed(105);
434     RightSpeed(75);
435   }
436 
437 }
438 
439 
440 
441 #endif
442 
443 
444 //Was implemented as update for interrupt driven wheel PID
445 void WheelSpeed(int LeftTarget, int RightTarget)
446 {
447   LEFT_SPEED = LeftTarget;
448   RIGHT_SPEED = RightTarget;
449 }
450 
451 #ifdef Wheel_Speed_TEST
452 
453 void main(void)
454 {
455   (void)printf("Starting");
456   InitPorts();
457   InitPWM();
458   InitTimers();
459 
460   //LeftSpeed(0);
461   //RightSpeed(50);
462   //(void)printf("-passed left right speed-");
463 
464 
465   while(1)
466   {
467   WheelSpeed(0,20);
468   //(void)printf("Left: Target: %6.3f, CurrSpeed: %6.3f, Int: %6.3f  Diff: %6.3f  Duty: %i \n\r",LEFT_SPEED/100.0, LCurrentSpeed, (LInt * _Ki_L), (LError * _Kp_L), LDutyCycle);
469   (void)printf("Right: Target: %6.3f, CurrSpeed: %6.3f, Int: %6.3f  Diff: %6.3f  Duty: %i \n\r",RIGHT_SPEED/100.0, RCurrentSpeed, (RInt * _Ki_R), (RError * _Kp_R), RDutyCycle);
470   //printf("L %u\n\r", LeftTicks);
471   //printf("R %u\n\r", RightTicks);
472   //printf("trhough");
473 
474   }
475 }
476 
477 #endif