// Gyro calculation Robot position in degrees = angle
		ang_vel = (gyro - gyro_zero)*200*3.14159/180;
		// Integration
		angle=-57.29578*(ang_vel+ang_vel_old)*.001/2+angle;
		
		if (angle > 180)
			angle = -180;
		if (angle < -180)
			angle = 180;
				
		ang_vel_old = ang_vel;
		
		
		VBangle_K[1] = john_vbangle;
		gyroK[1] = ang_vel; 

		
		//complimentary filter	
				
		VBangle_fK[2] = ( (T*T+2*tau*T)*VBangle_K[1] - 2*tau*T*VBangle_K[0] + (2*tau*tau+2*tau*T)*VBangle_fK[1] - tau*tau*VBangle_fK[0] )/ (T*T+2*tau*T+tau*tau);
		gyrofK[2] = ( gyroK[1]*tau*tau*T - gyroK[0]*tau*tau*T + (2*tau*tau+2*tau*T)*gyrofK[1] - tau*tau*gyrofK[0] )/ (T*T+2*tau*T+tau*tau);

		comp_angle = (VBangle_fK[2]+gyrofK[2]);
		
		// Up-date Filter Values
		
		VBangle_fK[0] = VBangle_fK[1];
		VBangle_fK[1] = VBangle_fK[2];
		
		VBangle_K[0] = VBangle_K[1];
			
		gyrofK[0]=gyrofK[1];
		gyrofK[1]=gyrofK[2];
	
		gyroK[0] =gyroK[1];


...


  	 	
   	 	VBangle = atan2((Gy-Ry),(Gx-Rx));
   		cur = VBangle*57.29578;
   		   	 		 		 		 		 	
   	 	if ((cur > 90) || (cur < -90)){
    		if (cur < 0 && prev > 0){
        		anglesum = anglesum + (180-prev) + (cur + 180);
        	}
    		else if (cur > 0 && prev < 0){
        		anglesum = anglesum - ( (180-cur) + (prev + 180) );
        	}
    		else{ 
        		anglesum = anglesum + (cur-prev);
        	}
        } else {
        	anglesum = anglesum + (cur-prev);
        }
   		
   		prev=cur;
   		
   	 
   	 	newanglesumflag = 1;