Diff: inits.h

Revision:

14:102a2b4f5c86

Parent:

13:40141b362092

Child:

16:e9945e3b4712

--- a/inits.h	Tue Oct 06 11:30:38 2015 +0000
+++ b/inits.h	Tue Oct 06 14:17:11 2015 +0000
@@ -1,16 +1,16 @@
 //****************************************************************************/
 // Defines
 //****************************************************************************/
-#define RATE  0.01 
-#define Kc    0.30
-#define Ti    0.20
+#define RATE  0.1 
+#define Kc    1.5
+#define Ti    0.8
 #define Td    0.0
 
 //****************************************************************************/
 // Globals
 //****************************************************************************/
 Serial pc(USBTX, USBRX);
-HIDScope    scope(3);        // Instantize a 2-channel HIDScope object
+HIDScope    scope(5);        // Instantize a 2-channel HIDScope object
 Ticker      scopeTimer;      // Instantize the timer for sending data to the PC 
 InterruptIn startButton(D3);
 InterruptIn stopButton(D2);
@@ -39,7 +39,8 @@
 Ticker motorControlTicker;
 bool goFlag=false;
 bool systemOn=false;
-// Working variables.
+
+// Working variables: motors
 volatile int leftPulses     = 0;
 volatile int leftPrevPulses = 0;
 volatile float leftPwmDuty  = 0.0;
@@ -52,9 +53,26 @@
 volatile float rightPwmDutyPrev = 0.0;
 volatile float rightVelocity = 0.0;
 
+// request calculation variables
 float request;
 float request_pos;
 float request_neg;
+float leftAngle;
+float rightAngle;
+float round=4200;
+float toX;
+float toY;
+float leftDeltaAngle;
+float rightDeltaAngle;
+float leftRequest;
+float rightRequest;
+float currentX;
+float currentY;
+float toLeftAngle;
+float toRightAngle;
+const double M_PI =3.141592653589793238463;
+const float l = 10; // distance between the motors
+const float armlength=15; // length of the arms from the motor
 
 void initMotors(){
     //Initialization of motor
@@ -64,7 +82,7 @@
     
     rightMotor.period_us(50);
     rightMotor = 0.0;
-    rightDirection = 1;
+    rightDirection = 0;
     
 }