control for robotic arm that can play chess using a granular gripper
Dependencies: Encoder mbed HIDScope Servo MODSERIAL
Fork of chessRobot by
Diff: actuators.cpp
- Revision:
- 93:a8898eb80edc
- Parent:
- 91:6bbbbc6643c8
- Child:
- 94:28e274481b60
- Child:
- 97:0f67952051e5
diff -r 6bbbbc6643c8 -r a8898eb80edc actuators.cpp --- a/actuators.cpp Wed Oct 21 10:13:10 2015 +0000 +++ b/actuators.cpp Wed Oct 21 13:16:24 2015 +0200 @@ -5,6 +5,7 @@ #include "encoder.h" #include "HIDScope.h" #include "Servo.h" +#include "buttons.h" // functions for controlling the motors bool motorsEnable = false; bool safetyOn = false; // start with safety off for calibration @@ -46,6 +47,15 @@ double timechange; bool pidOut = 0; + // for Calibration: + bool calibrating1 = true; + bool calibrating2 = false; + double motorCalSpeed = 10; // deg/sec + double returnSpeed = -10; + bool springHit = false; + float lastCall = 0; + + // Create object instances // Safety Pin DigitalIn safetyIn(safetyPin); @@ -167,32 +177,47 @@ } void calibrateMotors(){ -// safetyOn = false; -// bool calibrating1 = true; -// bool calibrating2 = true; -// double motorCalSpeed = 10; // deg/sec -// while (calibrating1 || calibrating2){ -// if (calibrating1){ -// motor1SetSpeed = motorCalSpeed; // deg/sec -// if(safetyIn.read() !=1){ // check if arm reached safety position -// calibrating1 = false; // done -// motor1SetSpeed = 0; // brake motor -// encoder1.setPosition(0); // set motor 1 cal angle -// // move away -// } -// } -// if (calibrating1){ -// motor2SetSpeed = motorCalSpeed; // deg/sec -// if(safetyIn.read() !=1){ // check if arm reached safety position -// calibrating2 = false; // done -// motor2SetSpeed = 0; // brake motor -// encoder2.setPosition(0); // set motor 2 cal angle -// // move away -// } -// } -// writeMotors(); -// wait(0.01f); -// } + safetyOn = false; + redLed.write(0); greenLed.write(1); blueLed.write(1); + while (calibrating1 || calibrating2){ + if (calibrating1){ + motor1SetSpeed = motorCalSpeed; // deg/sec + redLed.write(1); greenLed.write(0); blueLed.write(1); + if(safetyIn.read() !=1){ // check if arm reached safety position + encoder1.setPosition(0); // set motor 1 cal angle + motor1SetSpeed = returnSpeed; // move away + springHit = true; + }else{ + if(springHit){ // if hit and after is no longer touching spring + motor1SetSpeed = 0; + springHit = false; + calibrating1 = false; + calibrating2 = true; // start calibrating 2 + } + } + } + if (calibrating2){ + motor2SetSpeed = motorCalSpeed; + redLed.write(1); greenLed.write(1); blueLed.write(0); + if(safetyIn.read() !=1){ // check if arm reached safety position + encoder2.setPosition(0); // set motor 2 cal angle + motor2SetSpeed = returnSpeed; // move away + springHit = true; + }else{ + if(springHit){ // if hit and after is no longer touching spring + motor2SetSpeed = 0; + springHit = false; + calibrating2 = false; // stop calibrating 2 + } + } + } + now = t.read(); // call motor using timer instead of wait + if(now - lastCall > motorCall){ + writeMotors(); + lastCall = now; + } + + } safetyOn = true; // turn safety on after callibration }