Omnidirectional platform control

Ok, so the mega will be driving the platform itself 4 motors with that code. The 101 will be driving the robotic arm, and telescopic lift. So 6 servos + 2 linear actuators.

I included a wifi shield to use it to monitor the process once I start integrating sensors. For now I just want to achieve basic movements.

Btw that code is loaded into the mega right now, haven't attempted to do any handshaking yet although ive been reading a few tutorials about that.

Edit
"Hardware im using:

Mega board

http://www.robotshop.com/media/files/pdf/arduino-microcontroller-board-10009.pdf

The motor shield appears here

http://www.robotshop.com/media/files/pdf2/4wd_omni-directional_manual.pdf

The arduino 101

The servo shield

The motor shield

https://cdn-learn.adafruit.com/downloads/pdf/adafruit-motor-shield-v2-for-arduino.pdf"