PROS/ConVEX Toss Up Code 1

/*-----------------------------------------------------------------------------*/
/*                                                                             */
/*                        Copyright (c) James Pearman                          */
/*                                   2013                                      */
/*                            All Rights Reserved                              */
/*                                                                             */
/*-----------------------------------------------------------------------------*/
/*                                                                             */
/*    Module:     vexuser.c                                                    */
/*    Author:     James Pearman                                                */
/*    Created:    7 May 2013                                                   */
/*                                                                             */
/*    Revisions:                                                               */
/*                V1.00  04 July 2013 - Initial release                        */
/*                                                                             */
/*-----------------------------------------------------------------------------*/
/*                                                                             */
/*    The author is supplying this software for use with the VEX cortex        */
/*    control system. This file can be freely distributed and teams are        */
/*    authorized to freely use this program , however, it is requested that    */
/*    improvements or additions be shared with the Vex community via the vex   */
/*    forum.  Please acknowledge the work of the authors when appropriate.     */
/*    Thanks.                                                                  */
/*                                                                             */
/*    Licensed under the Apache License, Version 2.0 (the "License");          */
/*    you may not use this file except in compliance with the License.         */
/*    You may obtain a copy of the License at                                  */
/*                                                                             */
/*      http://www.apache.org/licenses/LICENSE-2.0                             */
/*                                                                             */
/*    Unless required by applicable law or agreed to in writing, software      */
/*    distributed under the License is distributed on an "AS IS" BASIS,        */
/*    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. */
/*    See the License for the specific language governing permissions and      */
/*    limitations under the License.                                           */
/*                                                                             */
/*    The author can be contacted on the vex forums as jpearman                */
/*    or electronic mail using jbpearman_at_mac_dot_com                        */
/*    Mentor for team 8888 RoboLancers, Pasadena CA.                           */
/*                                                                             */
/*-----------------------------------------------------------------------------*/

#include <stdlib.h>

#include "ch.h"         // needs for all ChibiOS programs
#include "hal.h"        // hardware abstraction layer header
#include "vex.h"        // vex library header

// Digi IO configuration
static  vexDigiCfg  dConfig[kVexDigital_Num] = {
        { kVexDigital_1,    kVexSensorDigitalOutput, kVexConfigOutput,      0 },
        { kVexDigital_2,    kVexSensorDigitalOutput, kVexConfigOutput,      0 },
        { kVexDigital_3,    kVexSensorDigitalInput,  kVexConfigInput,       0 },
        { kVexDigital_4,    kVexSensorDigitalInput,  kVexConfigInput,       0 },
        { kVexDigital_5,    kVexSensorDigitalInput,  kVexConfigInput,       0 },
        { kVexDigital_6,    kVexSensorDigitalInput,  kVexConfigInput,       0 },
        { kVexDigital_7,    kVexSensorDigitalInput,  kVexConfigInput,       0 },
        { kVexDigital_8,    kVexSensorDigitalInput,  kVexConfigInput,       0 },
        { kVexDigital_9,    kVexSensorDigitalInput,  kVexConfigInput,       0 },
        { kVexDigital_10,   kVexSensorDigitalInput,  kVexConfigInput,       0 },
        { kVexDigital_11,   kVexSensorDigitalInput,  kVexConfigInput,       0 },
        { kVexDigital_12,   kVexSensorDigitalInput,  kVexConfigInput,       0 }
};

static  vexMotorCfg mConfig[kVexMotorNum] = {
        { kVexMotor_1,      kVexMotor393T,           kVexMotorNormal,           kVexSensorNone,        0 },
        { kVexMotor_2,      kVexMotor393S,           kVexMotorNormal,           kVexSensorNone,        0 },
        { kVexMotor_3,      kVexMotor393S,           kVexMotorNormal,           kVexSensorNone,        0 },
        { kVexMotor_4,      kVexMotor393S,           kVexMotorNormal,           kVexSensorNone,        0 },
        { kVexMotor_5,      kVexMotor393S,           kVexMotorReversed,         kVexSensorNone,        0 },
        { kVexMotor_6,      kVexMotor393S,           kVexMotorNormal,           kVexSensorNone,        0 },
        { kVexMotor_7,      kVexMotor393S,           kVexMotorReversed,         kVexSensorNone,        0 },
        { kVexMotor_8,      kVexMotor393T,           kVexMotorReversed,         kVexSensorNone,        0 },
        { kVexMotor_9,      kVexMotor393T,           kVexMotorNormal,           kVexSensorNone,        0 },
        { kVexMotor_10,     kVexMotor393T,           kVexMotorReversed,         kVexSensorNone,        0 }
};

const int MOTOR_RIGHT_ARM = 1;
const int MOTOR_LEFT_1 = 2;
const int MOTOR_LEFT_2 = 3;
const int MOTOR_LEFT_3 = 4;
const int MOTOR_RIGHT_3 = 5;
const int MOTOR_RIGHT_2 = 6;
const int MOTOR_RIGHT_1 = 7;
const int MOTOR_INTAKE_RIGHT = 8;
const int MOTOR_INTAKE_LEFT = 9;
const int MOTOR_LEFT_ARM = 10;

const int SENSOR_PISTON_1 = 1;
const int SENSOR_PISTON_2 = 2;

/*-----------------------------------------------------------------------------*/
/** @brief      User setup                                                     */
/*-----------------------------------------------------------------------------*/
/** @details
 *  The digital and motor ports can (should) be configured here.
 */
void
vexUserSetup()
{
    vexDigitalConfigure( dConfig, DIG_CONFIG_SIZE( dConfig ) );
    vexMotorConfigure( mConfig, MOT_CONFIG_SIZE( mConfig ) );
}

/*-----------------------------------------------------------------------------*/
/** @brief      User initialize                                                */
/*-----------------------------------------------------------------------------*/
/** @details
 *  This function is called after all setup is complete and communication has
 *  been established with the master processor.
 *  Start other tasks and initialize user variables here
 */
void
vexUserInit()
{

}

/*-----------------------------------------------------------------------------*/
/** @brief      Autonomous                                                     */
/*-----------------------------------------------------------------------------*/
/** @details
 *  This thread is started when the autonomous period is started
 */
msg_t
vexAutonomous( void *arg )
{
    (void)arg;

    // Must call this
    vexTaskRegister("auton");

    while(1)
        {
        // Don't hog cpu
        vexSleep( 25 );
        }

    return (msg_t)0;
}

#define MotorDriveL     kVexMotor_1
#define MotorDriveR     kVexMotor_10

/*-----------------------------------------------------------------------------*/
/** @brief      Driver control                                                 */
/*-----------------------------------------------------------------------------*/
/** @details
 *  This thread is started when the driver control period is started
 */
msg_t
vexOperator( void *arg )
{
    int16_t     blink = 0;

    (void)arg;

    // Must call this
    vexTaskRegister("operator");



    // Run until asked to terminate
    while(!chThdShouldTerminate())
        {

        tankDrive(vexControllerGet(Ch3),vexControllerGet(Ch2));

        vexMotorSet(MOTOR_RIGHT_ARM,(vexControllerGet(Btn5U)-vexControllerGet(Btn5D))*127+16);
        vexMotorSet(MOTOR_LEFT_ARM,(vexControllerGet(Btn5U)-vexControllerGet(Btn5D))*127+16);

        vexMotorSet(MOTOR_INTAKE_RIGHT,(vexControllerGet(Btn6U)-vexControllerGet(Btn6D))*127+16);
        vexMotorSet(MOTOR_INTAKE_LEFT,(vexControllerGet(Btn6U)-vexControllerGet(Btn6D))*127+16);

        if(vexControllerGet(Btn8U)) {
            vexDigitalPinSet(SENSOR_PISTON_1,1);
            vexDigitalPinSet(SENSOR_PISTON_2,1);
        }
        if(vexControllerGet(Btn8D)) {
            vexDigitalPinSet(SENSOR_PISTON_1,0);
            vexDigitalPinSet(SENSOR_PISTON_2,0);
        }

        // Don't hog cpu
        vexSleep( 25 );
        }

    return (msg_t)0;
}

void
tankDrive(int left, int right) {

    vexMotorSet(MOTOR_LEFT_1,left);
    vexMotorSet(MOTOR_LEFT_2,left);
    vexMotorSet(MOTOR_LEFT_3,left);
    vexMotorSet(MOTOR_RIGHT_1,right);
    vexMotorSet(MOTOR_RIGHT_2,right);
    vexMotorSet(MOTOR_RIGHT_3,right);

}