Main Robot Code for VEX Team 3117

vw_full_code

// This code requires the vanwall library to function
#include "vanwall.c"

// =*=*= Main Robot Code for VEX Team 3117 =*=*=
//    *=       Release: 04/01/17           =*

// This file is composed of the following sections:
//   - Allocation : Creating data structures in memory
//   - Creation   : Initialising data structures
//                   * Assigning ports to motors
//                   * Assigning coefficients to motors
//                   * Binding motors
//   - Mainloop   : Polling input and updating motors

// =*=*= Motor Allocation: Drive =*=*=
Motor fl; // Front Left  wheel motor
Motor fr; // Front Right wheel motor
Motor rl; // Rear  Left  wheel motor
Motor rr; // Rear  Right wheel motor

LinearFourDrive drive;

// =*=*= Motor Allocation: Arm =*=*=
Motor arm           ; // Virtual Motor represents entire Arm mechanism

Motor top_single    ; // The single (non y-cabled) motor  on the top    row  
Motor top_pair      ; // The paired (y-cabled)     motors on the top    row
Motor bottom_single ; // The single (non y-cabled) motor  on the bottom row
Motor bottom_pair   ; // The paired (y-cabled)     motors on the bottom row

// =*=*= Motor Allocation: Claw =*=*=
Motor claw    ; // Virtual Motor represents entire Claw mechanism

Motor claw_one; // The first  claw motor
Motor claw_two; // The second claw motor

task main(){
  // =*=*= Create the Drive =*=*=
	create_motor(&fl, TORQUE, port3, STANDARD);
	create_motor(&rl, TORQUE, port8, STANDARD);
	
	create_motor(&fr, TORQUE, port2, FLIPPED );
	create_motor(&rr, TORQUE, port9, FLIPPED );
	
	create_linear_four(&drive, &fl, &fr, &rl, &rr);
	
	// =*=*= Create the Arm =*=*=
	create_motor(&top_pair     , TORQUE, port4, FLIPPED );
	create_motor(&top_single   , TORQUE, port5, STANDARD);
	create_motor(&bottom_single, TORQUE, port6, STANDARD);
	create_motor(&bottom_pair  , TORQUE, port7, FLIPPED );
	
	create_virtual_motor(&arm, TORQUE, (tMotor) NULL, STANDARD);
	
	// Bind chain: arm -> top_pair -> top_single -> bottom_single -> bttom_pair
	set_bound_motor(&arm           , &top_pair      ); 
	set_bound_motor(&top_pair      , &top_single    );
	set_bound_motor(&top_single    , &bottom_single );
	set_bound_motor(&bottom_single , &bottom_pair   );
	
	// =*=*= Create the Claw =*=*=
	create_motor(&claw_one, TORQUE, port1 , STANDARD);
	create_motor(&claw_two, TORQUE, port10, STANDARD);
	
	create_virtual_motor(&claw, TORQUE, (tMotor) NULL, STANDARD);
	
	// Bind chain: claw -> claw_one -> claw_two
	set_bound_motor(&claw    , &claw_one);
	set_bound_motor(&claw_one, &claw_two);
	
	// =*=*= Mainloop: poll input, update motors =*=*=
	while (true) {
	    control_motor_digital     (&arm      , joystick(Btn5U) , joystick(Btn5D), 100);		  
		  control_motor_digital     (&claw     , joystick(Btn6U) , joystick(Btn6D), 127);
  		control_linear_four_arcade(&drive    , joystick(Ch3)   , joystick(Ch1)       );
  		
	  	lcd_display_linear_four   (&drive);
	}
}