1 //Movement
  2 
  3 //#define TEST
  4 //#define TEST2
  5 //#define TEST3
  6 //#define TEST4
  7 
  8 
  9 /////////////////////////////////////////////////////////////////////
 10 //Functions built to be inserted into loops.
 11 //Return status=1 for functions that are still active
 12 //IMPLEMENTATION:
 13 //When the function is complete, gaurd against calling it again
 14 //
 15 //
 16 /////////////////////////////////////////////////////////////////////
 17 
 18 
 19 
 20 
 21 #include <hidef.h>      /* common defines and macros */
 22 #include <mc9s12e128.h>     /* derivative information */
 23 #include <stdio.h>
 24 #include <ADS12.h>
 25 #include <stdlib.h>
 26 #include <S12E128bits.h> /* E128 bit definitions  */
 27 #include <bitdefs.h> /* BIT0HI, BIT0LO....definitions */
 28 #include <stdlib.h>
 29 #include <math.h>
 30 
 31 #include "DeadReckoning.h"
 32 #include "Sensors.h"
 33 #include "Movement.h"
 34 
 35 #include "Timers.h"
 36 #include "PWM.h"
 37 #include "InitPorts.h"
 38 #include "Constants.h"
 39 #include "SPI.h"
 40 #include "ChkEvents.h"
 41 
 42 #define THRESH_ANGLE 10   //Angle is how close the bot needs to be to stop correcting b/c its 'close enough'
 43 #define PROXY_THRESH_POS .1
 44 #define PID_THRESH_POS 1
 45 
 46 
 47 #define K_INT_POS 0.00005 //Integral co-efficient for PID position adjust
 48 #define K_DELTA_POS .0002 //Delta co-efficient for PID position adujst
 49 
 50 #define K_INT_ANG .00000005 //Integral co-efficient for PID angle adjust
 51 #define K_DELTA_ANG .001 //Delta co-efficient for PID angle adjust
 52 
 53 void LeftFor(char);
 54 void RightFor(char);
 55 void LeftBack(char);
 56 void RightBack(char);
 57 void LeftStop(void);
 58 void RightStop(void);
 59 
 60 
 61 //******************Begin Module code****************************
 62 
 63 
 64 //Input: The direction you want to face
 65 //Return: Status marker (1 is still rotating, 0 if done)
 66 char SettingAngle(int TargetAngle){
 67   static float  Integral=0;
 68 
 69   int CurrentAngle;
 70   float Delta;
 71   float Speed;
 72 
 73   CurrentAngle = GetAngle();
 74   Delta = (TargetAngle - CurrentAngle);
 75 
 76   if(Delta > THRESH_ANGLE)
 77   {
 78     //Slow rotation speed to avoid overshoot: Angles are very sensitive
 79     WheelSpeed(-10, 10);
 80     //(void)printf("greater than thresh\r\n");
 81     return 1;
 82   }
 83 
 84   if(Delta < -THRESH_ANGLE)
 85   {
 86     //Slow rotation speed to avoid overshoot: Angles are very sensitive
 87     WheelSpeed(10, -10);
 88     //(void)printf("less than thresh\r\n");
 89     return 1;
 90   }
 91 
 92   WheelSpeed(0,0);
 93 
 94   (void)printf("Reached Thresh\r\n");
 95   (void)printf("DELTA: %6.4f\n\r", Delta);
 96   return 0;
 97 
 98 }
 99 
100 char RotateToBeacon()
101 {
102     PTIAD &= BIT4LO;
103     LeftSpeed(-97);
104     RightSpeed(100);
105     if((PTIAD & BIT5HI) == 1)
106     {
107         PTIAD |= BIT4HI; // Light LED
108         LeftSpeed(0);
109         RightSpeed(0);
110         return 1;
111     }
112     else
113     {
114         return 0;
115     }
116 }
117 
118 
119 ////////////////////////////////////////////////////////////////////
120 //Tape Following
121 //Puts you at a certain PID controlled position on the line of tape
122 char DrivingTape(float Speed)
123 {
124   char static ResetReckoning;
125   int Tape1,Tape2,Tape3,     Tape7,Tape8,Tape9;
126 
127 
128     //FORWARDS (It only drives forward now)
129     Tape1 = CheckTape(1); //Right side
130     Tape3 = CheckTape(3); //Left Side
131 
132    if(Tape1 == 0 && Tape3 == 0 ) //We go straight on the line
133     {
134       WheelSpeed(30,30);
135 
136       //(void)printf("\r\nStraight");
137       return 1;
138     }
139 
140     if(Tape1 == 1 && Tape3 == 0 ) //If Right ones on only
141     {
142       //Veer Right
143       WheelSpeed(30,29); //30, 20
144 
145       //(void)printf("\r\nTurn Right");
146       ResetReckoning = 1;
147       return 1;
148     }
149 
150     if(Tape1 == 0 && Tape3 == 1 ) //If Left ones on only
151     {
152       //Veer Left
153       WheelSpeed(29,30);  //20, 30
154 
155       //(void)printf("\r\n           Turn Left");
156       ResetReckoning = 1;
157       return 1;
158     }
159 
160 }
161 
162 /////////////////////////////////////////////////////////////////
163 //Returns Angle You should Rotate to aim correctly
164 
165 //Uses some trig to and notions of bot location and target locations in order to calculate the angle
166 //that bot needs to face (used for Rotating to Target and Home)
167 
168 //Although angles are returned in encoder ticks, they are internally dealt with in radians
169 //for  compatability with C library function
170 int CalculateTargetAngle(void)
171 {
172   float ArcTan, ArcSin, RadsToTicks;
173   int TargetAngle;
174   float DeltaX, DeltaY, Distance, Correction;
175   Location TargetLocation;
176   //TC_Answer_t CurrentTarget;
177   int CurrentTarget;
178 
179   CurrentTarget = GetTarget();
180   TargetLocation = GetTargetLocation(CurrentTarget);
181 
182 
183   (void)printf("Target#: %i, TargetX:%6.4f  TargetY:%6.4f  CurrentX:%6.4f  CurrentY:%6.4f  ", CurrentTarget, TargetLocation.x, TargetLocation.y, GetXpos(), GetYpos());
184 
185 
186   DeltaX = TargetLocation.x - GetXpos();
187   DeltaY = TargetLocation.y - GetYpos();
188 
189   (void)printf("deltax = %f   deltay = %f\r\n", DeltaX, DeltaY);
190 
191   Distance = sqrt(DeltaX*DeltaX + DeltaY*DeltaY);
192 
193   (void)printf("Inside Asin: %f\n\r", (Distance/GUN_TO_CENTER) );
194 
195   //Get Angle (assuming quadrants 1 or IV)
196   ArcTan =atan(DeltaY/DeltaX); //This atan(2/24) is an imperical correction term (presumed cause - Tape following or gun error)
197 
198   //Despite our best efforts to have all mathematics generally applicable, in practice there were small,
199   //but consistent errors, which we dealt with via these small correction terms
200   switch (CurrentTarget)
201   {
202     case 0:
203       Correction = -atan(2.0/24.0);
204       break;
205     case 1:
206       Correction = -atan(1.5/28);
207       break;
208     case 2:
209       Correction = 0;
210       break;
211     case 3:
212       Correction = 0;
213       break;
214     case 4:
215       Correction = -atan(1/60);
216       break;
217     case 5:
218       Correction = 0;
219       break;
220     case 6:
221       Correction = 0;
222       break;
223     case 7:
224       Correction = +atan(2.0/75.0);
225       break;
226   }
227 
228   ArcTan = ArcTan + Correction;
229 
230 
231   ArcSin = asin(GUN_TO_CENTER/Distance);  //This is a correction term, since the gun is not in the bot center
232   RadsToTicks = TicksPerRotation / (2*PI);
233 
234 
235   //Targets 16 & 17 are the Green and Red bases, however they were included in the Targets list
236   //so that this function could be simply applied for the return home state
237   if(CurrentTarget == 16  || CurrentTarget == 17)
238   {
239     TargetAngle = (int)(ArcTan*RadsToTicks);
240     (void)printf("Calculating Home Angle...Before phase correction: %6.4f\n\r",(TargetAngle/RadsToTicks) );
241   }
242   else
243   {
244     TargetAngle =  (int)( (ArcTan - ArcSin)*RadsToTicks ) - TicksPerRotation/4;
245     (void)printf("Calculating Target Angle...Before phase correction: %6.4f\n\r",(TargetAngle*360.0/TicksPerRotation) );
246 
247   }
248 
249 
250   //If angle is in quadrants II or III, we need to correct by a phase factor
251   if(DeltaX < 0){
252     if(DeltaY > 0){
253       //TargetAngle += (float)TicksPerRotation/2;  //Quadrant II
254       TargetAngle = TicksPerRotation/2 + TargetAngle;  //Quadrant II
255     }
256     else{
257 
258       //TargetAngle -= (float)TicksPerRotation/2;  //Quadrant III
259       TargetAngle = -TicksPerRotation/2 + TargetAngle;  //Quadrant III
260     }
261   }
262 
263   (void)printf("ArcTan: %6.4f,   ArcSin: %6.4f,  TargetAngle: %6.4f\n\r", ArcTan,ArcSin,(TargetAngle*(360.0/TicksPerRotation) ) );
264 
265   return TargetAngle;
266 }
267 
268 
269 ////////////////////////////////////////////////////////////////
270 //Setting Wheel Speeds
271 //Dont use this function except inside PID
272 //This is a direct PWM control
273 
274 void LeftSpeed(char Duty)
275 {
276   if(Duty > 0)
277   {
278     LeftFor(Duty);
279   }
280   if(Duty < 0)
281   {
282     LeftBack((char)abs(Duty));
283   }
284   if(Duty == 0)
285   {
286     LeftStop();
287   }
288 }
289 
290 void RightSpeed(char Duty)
291 {
292   if(Duty > 0){
293     RightFor(Duty);
294   }
295   if(Duty < 0)
296   {
297     RightBack((char)abs(Duty));
298   }
299   if(Duty == 0)
300   {
301     RightStop();
302   }
303 }
304 
305 
306 //////////////////////////////////////////////////////////////////
307 //Internal Functions
308 
309 //These functions are written seperate from LeftSpeed and RightSpeed because of the added complexity
310 //of setting polarity and the directional bit
311 
312 
313 void RightFor(char Duty){
314   //(void)printf("->Right Forward; Duty: %i\n\r", Duty);
315   PWMPOL |= _S12_PPOL0; //Sets polarity high duty cycle for PWM1
316   PWMDTY0 = (char)((FULL_DUTY*Duty)/100);
317   PTP = PTP & BIT2LO;   //Direction to Forward
318 }
319 void RightBack(char Duty){
320   //(void)printf("->Right Backward; Duty: %i\n\r", Duty);
321   PWMPOL &= ~_S12_PPOL0;   //Sets polarity low cycle for PWM1
322   PWMDTY0 = (char)(FULL_DUTY*Duty/100);
323   PTP = PTP | BIT2HI;     //Direction to Reverse
324 
325 }
326 void RightStop(void)
327 {
328 
329   //(void)printf("->Right Stop\n\r");
330   PWMDTY0 = 0;
331   //Stop Right Wheel
332 }
333 
334 
335 void LeftBack(char Duty){
336   //(void)printf("->Left Backward; Duty: %i\n\r", Duty);
337   PWMPOL |= _S12_PPOL1;   //Sets polarity high duty cycle for PWM0
338   PWMDTY1 = (char)((FULL_DUTY*(Duty))/100);
339   PTP = PTP & BIT1LO;     //Sets Direction Forward
340 
341 }
342 void LeftFor(char Duty){
343   //(void)printf("->Left Forward; Duty: %i\n\r", Duty);
344   PWMPOL &= ~_S12_PPOL1;   //Sets polarity low duty cycle for PWM0
345   PWMDTY1 = (char)((FULL_DUTY*Duty)/100);
346   PTP = PTP | BIT1HI;     //Sets direction to Reverse
347 }
348 void LeftStop(void)
349 {
350   //(void)printf("->Left Stop\n\r");
351   PWMDTY1 = 0;
352 }
353 
354 
355 /////////////////////////////Test Harnesses//////////////////////////////////
356 
357 
358 #ifdef TEST
359 void main(void)
360 {
361   float Ang,X=0,Y=0;
362   int KeyStroke, TargetAngle=(TicksPerRotation/1.1);
363   char Active =1, Reset=1;
364 
365   InitPorts();
366   InitPWM();
367   InitTimers();
368 
369   ReckonAngle(0);
370   ReckonXpos(0);
371   ReckonYpos(0);
372 
373   (void)printf("Starting\n\r");
374   (void)printf("Starting Position: %f\r\n", GetXpos());
375   (void)printf("K_INT_ANG: %f    K_DELTA_ANG:  %f\n\r", K_INT_ANG, K_DELTA_ANG);
376 
377   //while(SettingAngle(45*TicksPerRotation) != 0)
378   while(1)
379   {
380 
381   SettingAngle(TicksPerRotation/2);
382     PositionMath();
383     //SettingAngle(90);
384     //DrivingTape();
385   }
386 }
387 #endif
388 
389 
390   /*
391   //Remote Control
392     if ( kbhit() != 0){
393 
394       KeyStroke = getchar();
395       switch (KeyStroke)
396       {
397         case 'r':  //Reset
398         (void)printf("Resetting Angle");
399           Active = 1;
400           break;
401         case '8':  //Forward
402           (void)printf("Moving Forward\n\r");
403           RightSpeed(100);
404           LeftSpeed(100);
405           break;
406         case '2':  //Backward
407           (void)printf("Moving Backward\n\r");
408           RightSpeed(-100);
409           LeftSpeed(-100);
410           break;
411         case '4':  //Left
412           (void)printf("Rotating Left\n\r");
413           RightSpeed(100);
414           LeftSpeed(-100);
415           break;
416         case '6':   //Right
417           (void)printf("Rotating Right\n\r");
418           RightSpeed(-100);
419           LeftSpeed(100);
420           break;
421         case '5':   //Stop
422           (void)printf("Stopped\n\r");
423           RightSpeed(0);
424           LeftSpeed(0);
425           break;
426         case 'u':   //Update
427           (void)printf("Position update:\n\r");
428           Ang = GetAngle();
429           X = GetXpos();
430           Y = GetYpos();
431           (void)printf("Angle: %f, X-position: %f, Y-position: %f\n\r",Ang,X,Y);
432           break;
433         case '9':   //Right Wheel Forward Only
434           (void)printf("Right Wheel Forward Only\n\r");
435           RightSpeed(100);
436           LeftSpeed(0);
437           break;
438         case '7':   //Left Wheel Forward Only
439           (void)printf("Left Wheel Forward Only\n\r");
440           RightSpeed(0);
441           LeftSpeed(100);
442           break;
443         case '3':   //Right Wheel Backward Only
444           (void)printf("Right Wheel Backward Only\n\r");
445           RightSpeed(-100);
446           LeftSpeed(0);
447           break;
448         case '1':   //Left Wheel backward Only
449           (void)printf("Left Wheel backward Only\n\r");
450           RightSpeed(0);
451           LeftSpeed(-100);
452           break;
453         case '+': //increase cannon power
454           break;
455         case '-': //decrease cannon power
456           break;
457         case 'f':  //fire
458           break;
459       }  //end switch
460     }//end if
461 
462   //} //end while
463 //} //End testing main
464 */
465 
466 
467 #ifdef TEST2
468 void main(void)
469 {
470 
471 
472   InitPorts();
473   InitPWM();
474   InitTimers();
475 
476   (void)printf("Sensor:  1  2  3  5  7  8  9\n\r");
477   //LeftSpeed(100);
478   //RightSpeed(100);
479   while(1)
480   {
481   (void)printf("\r\n Value:  %i  %i  %i  %i  %i  %i  %i", CheckTape(1), CheckTape(2), CheckTape(3), CheckTape(5), CheckTape(7), CheckTape(8), CheckTape(9));
482     DrivingTape(1);
483   }
484 }
485  #endif
486 
487 #ifdef TEST3
488 void main(void)
489 {
490 
491 
492   InitPorts();
493   InitPWM();
494   InitTimers();
495 
496   (void)printf("Sensor:  1  2  3  5  7  8  9\n\r");
497   PWMDTY1 = 60;
498   PWMDTY2 = 60;
499   while(1)
500   {
501   //(void)printf("\r\n Value:  %i  %i  %i  %i  %i  %i  %i", CheckTape(1), CheckTape(2), CheckTape(3), CheckTape(5), CheckTape(7), CheckTape(8), CheckTape(9));
502   //  DrivingTape(1);
503   }
504 }
505  #endif
506 
507 
508  #ifdef TEST4
509 
510  void main(void)
511  {
512   float Ang,X=0,Y=0;
513   int KeyStroke, TargetAngle=(TicksPerRotation/1.1);
514   char Active =1, Reset=1;
515   int angle;
516 
517   InitConstants();
518 
519   ReckonAngle(0);
520   ReckonXpos(0);
521   ReckonYpos(0);
522 
523   angle = CalculateTargetAngle();
524 
525   printf("the Angle is: %i ", angle);
526 
527  }
528  #endif