Info:
I am powering the board with the wall-wart. The board is also sending serial output to an Arduino UNO.
Code:
Arm.h
[code]#ifndef ARM_H
#define ARM_H
#include “Arduino.h”
#include <Servo.h>
#include <PS2X_lib.h>
#define DISPLACE 3
class Arm{
private:
void servo1(int);
void servo2(int);
void servo3(int);
void servo4(int);
void servo5(int);
void servo6(int);
void servo7(int);
enum {maxservos = 7};
int pin[maxservos];
int pos[maxservos-1];
Servo servo[maxservos];
PS2X controller;
void calibrate();
void deflt();
public:
Arm();
void prep();
void run();
};
#endif[/code]
Arm.cpp
[code]#include “Arm.h”
Arm::Arm(){}
void Arm::prep(){
Serial.begin(9600);
pin[0] = 2;
pin[1] = 3;
pin[2] = 4;
pin[3] = 5;
pin[4] = 10;
pin[5] = 11;
pin[6] = 13;
servo[0].attach(pin[0]);
servo[1].attach(pin[1]);
servo[2].attach(pin[2]);
servo[3].attach(pin[3]);
servo[4].attach(pin[4]);
servo[5].attach(pin[5]);
servo[6].attach(pin[6]);
controller.config_gamepad(9,7,8,6,false,false);
calibrate();
}
void Arm::run(){
controller.read_gamepad();
if(controller.Analog(PSS_LY) > 129){
servo1(DISPLACE);
}
if(controller.Analog(PSS_LY) < 125){
servo1(-DISPLACE);
}
if(controller.Analog(PSS_RY) > 125){
servo2(DISPLACE);
}
if(controller.Analog(PSS_RY) < 129){
servo2(-DISPLACE);
}
if(controller.Analog(PSS_LX) > 128){
servo3(DISPLACE);
}
if(controller.Analog(PSS_LX) < 126){
servo3(-DISPLACE);
}
if(controller.Button(PSB_PAD_UP)){
servo4(DISPLACE);
}
if(controller.Button(PSB_PAD_DOWN)){
servo4(-DISPLACE);
}
if(controller.Button(PSB_L1)){
servo5(DISPLACE);
}
if(controller.Button(PSB_R1)){
servo5(-DISPLACE);
}
if(controller.Button(PSB_CROSS)){
servo6(DISPLACE);
}
if(controller.Button(PSB_TRIANGLE)){
servo6(-DISPLACE);
}
if(controller.Button(PSB_SQUARE)){
servo7(DISPLACE);
}
if(controller.Button(PSB_CIRCLE)){
servo7(-DISPLACE);
}
if(controller.Button(PSB_L2)){
calibrate();
}
if(controller.Button(PSB_R2)) {
deflt();
}
}
void Arm::servo1(int a){
servo[0].write(servo[0].read() + a);
}
void Arm::servo2(int a){
servo[1].write(servo[1].read() + a);
}
void Arm::servo3(int a){
servo[2].write(servo[2].read() + a);
}
void Arm::servo4(int a){
servo[3].write(servo[3].read() + a);
}
void Arm::servo5(int a){
servo[4].write(servo[4].read() + a);
}
void Arm::servo6(int a){
servo[5].write(servo[5].read() + a);
}
void Arm::servo7(int a){
servo[6].write(servo[6].read() + a);
}
void Arm::calibrate(){
pos[0] = servo[0].read();
pos[1] = servo[1].read();
pos[2] = servo[2].read();
pos[3] = servo[3].read();
pos[4] = servo[4].read();
pos[6] = servo[6].read();
}
void Arm::deflt(){
if(servo[0].read() != pos[0]) servo[0].write(pos[0]);
if(servo[1].read() != pos[1]) servo[1].write(pos[1]);
if(servo[2].read() != pos[2]) servo[2].write(pos[2]);
if(servo[3].read() != pos[3]) servo[3].write(pos[3]);
if(servo[4].read() != pos[4]) servo[4].write(pos[4]);
if(servo[6].read() != pos[6]) servo[6].write(pos[6]);
}[/code]
CSRIVBB.ino
[code]#include <Drive.h>
#include <PS2X_lib.h>
#include <Servo.h>
#include <Arm.h>
Arm arm;
DriveBB drive;
PS2X controller;
boolean n = false;
void setup(){
arm.prep();
drive.prep();
controller.config_gamepad(9,7,8,6,false,false);
}
void loop(){
controller.read_gamepad();
if(n == true){
drive.run();
}
else{
arm.run();
}
if(controller.ButtonPressed(PSB_SELECT)){
n = !n;
}
delay(50);
}[/code]