Diff: inits.h

Revision:

3:70f78cfc0f25

Parent:

2:8f9b6c1e89cf

Child:

6:ae2ce89dd695

--- a/inits.h	Tue Oct 13 18:54:38 2015 +0000
+++ b/inits.h	Wed Oct 14 13:16:35 2015 +0000
@@ -1,22 +1,15 @@
-//****************************************************************************/
-// Defines
-//****************************************************************************/
+
 #define RATE  0.005f 
 #define Kc    0.30
 #define Ti    0.00
 #define Td    0.0
 
-//****************************************************************************/
-// Globals
-//****************************************************************************/
+///////////////////////
+/// pin layout ////////
+///////////////////////
+
 Serial pc(USBTX, USBRX);
-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);
-//--------
-// Motors 
-//--------
+
 // Left motor.
 PwmOut leftMotor(D5);
 DigitalOut leftDirection(D4);
@@ -30,27 +23,41 @@
 PID rightController(Kc, Ti, Td, RATE);
 
 // edge leads
-DigitalOut outofboundsled(LED1);
-DigitalIn edgeleft(D0);
-DigitalIn edgeright(D1);
+DigitalIn edgeleft(PTC5);
+DigitalIn edgeright(PTC7);
+DigitalIn edgetop(PTC0);
+DigitalIn edgebottom(PTC9);
 
-
-// EMG input
+// emg input
 AnalogIn pot1(A0);
 AnalogIn pot2(A1);
-biquadFilter leftVelocityfilter((double) -1.561018075800718, (double) 0.641351538057563, (double) 0.020083365564211, (double) 0.040166731128423, (double) 0.020083365564211);
-biquadFilter rightVelocityfilter((double) -1.561018075800718, (double) 0.641351538057563, (double) 0.020083365564211, (double) 0.040166731128423, (double) 0.020083365564211);
-// Timers
+
+// user interface
+DigitalOut outofboundsled(LED1);
+InterruptIn startButton(D3);
+InterruptIn stopButton(D2);
+HIDScope scope(5);
+
+////////////////////////////
+//////// Timers ////////////
+////////////////////////////
+Ticker scopeTimer;
 Timer endTimer;
 Ticker motorControlTicker;
 bool goFlag=false;
 bool systemOn=false;
-// Working variables.
+
+
+////////////////////////////
+///////// Variables ////////
+////////////////////////////
+
+
+// Working variables for motors
 volatile int leftPulses     = 0;
 volatile float leftAngle=0;
 volatile int leftPrevPulses = 0;
 volatile float leftPwmDuty  = 0.0;
-volatile float leftPwmDutyPrev = 0.0;
 volatile float leftVelocity = 0.0;
 float leftRequest=0;
 
@@ -58,15 +65,16 @@
 volatile int rightPrevPulses = 0;
 volatile float rightAngle =0;
 volatile float rightPwmDuty  = 0.0;
-volatile float rightPwmDutyPrev = 0.0;
 volatile float rightVelocity = 0.0;
 float rightRequest=0;
 
-float request;
-float request_pos;
-float request_neg;
+// working vars for calculation
+float horrequest;
+float verrequest;
+float grenslaag=0.3;
+float grenshoog=0.7;
 float round=27720;
-float maxspeed=10; //cm/sec
+float maxspeed=1; //cm/sec
 float currentX;
 float currentY;
 float l=10;
@@ -74,10 +82,11 @@
 float toY;
 float toLeftAngle;
 float toRightAngle;
-float leftDeltaAngle;
-float rightDeltaAngle;
 float M_PI=3.14159265359;
 
+//////////////////////////////////
+////// initialize functions //////
+//////////////////////////////////
 
 void initMotors(){
     //Initialization of motor
@@ -93,13 +102,13 @@
 
 void initPIDs(){
     //Initialization of PID controller
-    leftController.setInputLimits(-180.0, 180.0);
+    leftController.setInputLimits(-90.0, 90.0);
     leftController.setOutputLimits(-1.0, 1.0);
     leftController.setBias(0);
     leftController.setDeadzone(-0.4, 0.4);
     leftController.setMode(AUTO_MODE);
     
-    rightController.setInputLimits(-180.0, 180.0);
+    rightController.setInputLimits(-90.0, 90.0);
     rightController.setOutputLimits(-1.0, 1.0);
     rightController.setBias(0);
     rightController.setDeadzone(-0.4, 0.4);