I did a total makeover, through away FreeRTOS and changed everything with the standard library 'Scheduler'.
By reading the whole documentation I understood that it isn't a real 'realtime system' . By every delay in the task it starts another task, this feels like doing things at the same time, but it isn't. You can't, so I disayded to try it on another way. I readed the instructions of 'Scheduler' and saw it was on the same way but in a lot simpler way.
I was very suprised of the result, it is a lot better. The benefit is working with a arduino due, it is a lot powerfuller then the mega board.

This is what I did,

#include <Scheduler.h>
void setup()
{
	Scheduler.startLoop(vStp01TaskON); // Write data to display
	Scheduler.startLoop(vStp02TaskON); // Read data from display
	Scheduler.startLoop(vStp03TaskON); // set position steppers
	Scheduler.startLoop(vStp04TaskON); // stepper 1
	Scheduler.startLoop(vStp05TaskON); // stepper 2
	Scheduler.startLoop(vStp06TaskON); // stepper 3
	Scheduler.startLoop(vStp07TaskON); // stepper 4
	Scheduler.startLoop(vStp08TaskON); // stepper 5
	Scheduler.startLoop(vStp09TaskON); // stepper 6
	Scheduler.startLoop(vStp10TaskON); // stepper 7
}

/*-----------------------------------------------------------*/
void loop()
{
	delay(1); // 1ms seconds waiting
}
/*-----------------------------------------------------------*/
void vStp01TaskON()
{
		digitalWrite(leds[4], task1 = !task1);
		SerialWrite();
		delay(10);  // 10ms seconds waiting
}
/*-----------------------------------------------------------*/
void vStp02TaskON()
{
		SerialRead();
		delay(10);  // 10ms seconds waiting
}
/*-----------------------------------------------------------*/
void vStp03TaskON()
{
		position();
		delay(10);  // 10ms seconds waiting
}
/*-----------------------------------------------------------*/
void vStp04TaskON()
{
		Stepper1Routine();
		delay(1); // 1ms seconds waiting
}
/*-----------------------------------------------------------*/
void vStp05TaskON()
{
		Stepper2Routine();
		delay(1); // 1ms seconds waiting
}
/*-----------------------------------------------------------*/
void vStp06TaskON()
{
		Stepper3Routine();
		delay(1); // 1ms seconds waiting
}
/*-----------------------------------------------------------*/
void vStp07TaskON()
{
		Stepper4Routine();
		delay(1); // 1ms seconds waiting
}
/*-----------------------------------------------------------*/
void vStp08TaskON()
{
		Stepper5Routine();
		delay(1); // 1ms seconds waiting
}
/*-----------------------------------------------------------*/
void vStp09TaskON()
{
		Stepper6Routine();
		delay(1); // 1ms seconds waiting
}
/*-----------------------------------------------------------*/
void vStp10TaskON()
{
		// Gripper & light
//		Scheduler.delay(1); // 2ms seconds waiting
		yield();
}
/*-----------------------------------------------------------*/