Dammenisvoorcasuals
control for robotic arm that can play chess using a granular gripper
Dependencies: Encoder mbed HIDScope Servo MODSERIAL
Fork of
chessRobot
by 
a steenbeek
Revision 127:831f03471efb, committed 2015-10-29
- Comitter:
- annesteenbeek
- Date:
- Thu Oct 29 22:46:30 2015 +0100
- Parent:
- 126:56866cefaa08
- Child:
- 128:c583aff5a7bf
- Commit message:
- added web interface components and flags for enabeling functionality
Changed in this revision
--- a/actuators.cpp Thu Oct 29 20:06:41 2015 +0000
+++ b/actuators.cpp Thu Oct 29 22:46:30 2015 +0100
@@ -80,6 +80,10 @@
PID motor1PID(&motor1Speed, &motor1PWM, &motor1SetSpeed, Kp1, Ki1, Kd1);
PID motor2PID(&motor2Speed, &motor2PWM, &motor2SetSpeed, Kp2, Ki2, Kd2);
+// set pumpswitch
+DigitalOut pumpSwitch(pumpPin);
+
+
Timer t;
void motorInit(){
@@ -110,7 +114,7 @@
void motorControl(){
- // EMG signals to motor speeds
+ // EMG signals to motor speeds
if(!usePotmeters && controlAngle){
double scaleVel = 20;
motor1SetSpeed = x_velocity*scaleVel;
@@ -165,7 +169,7 @@
void writeMotors(){
motor1PID.Compute(); // calculate PID outputs, output changes automatically
motor2PID.Compute();
-// write new values to motor's
+ // write new values to motor's
if (motor1SetSpeed ==0 ){
motor1PID.SetOutputLimits(0,0);
}
@@ -285,29 +289,32 @@
bool kinematics(){
- // calculate current x and Y
- Xpos = (L2+L3)*cos((motor1Pos + motor2Pos)*PI/180) + L1*cos(motor1Pos*PI/180);
- Ypos = (L2+L3)*sin((motor1Pos + motor2Pos)*PI/180) + L1*sin(motor1Pos*PI/180);
- // check if x and y are within limits
- // else Store the constraint line
- // check if movement is in direction of constraint
- // else return false no movement (anglespeed = 0)
- // calculate required angle speeds
- if( (Xpos>Xmax && setXSpeed > 0 )|| \
- (Xpos<Xmin && setXSpeed < 0 )|| \
- (Ypos>Ymax && setYSpeed > 0 )|| \
- (Ypos<Ymin && setYSpeed < 0 ) \
- ){
- motor1SetSpeed = 0;
- motor2SetSpeed = 0;
- return false;
- }
-motor1SetSpeed = (180/PI)*((setXSpeed*cos((motor1Pos + motor2Pos)*PI/180) + \
- setYSpeed*sin((motor1Pos + motor2Pos)*PI/180))/(L1*sin(motor2Pos*PI/180)));
-motor2SetSpeed = (180/PI)*(-(setXSpeed*(L2+L3)*cos((motor1Pos + motor2Pos)*PI/180) + \
- setYSpeed*(L2+L3)*sin((motor1Pos + motor2Pos)*PI/180) + \
- setXSpeed*L1*cos(motor1Pos*PI/180) + \
- setYSpeed*L1*sin(motor1Pos*PI/180))/(L1*(L2+L3)*sin(motor2Pos*PI/180)));
+ // calculate current x and Y
+ Xpos = (L2+L3)*cos((motor1Pos + motor2Pos)*PI/180) + L1*cos(motor1Pos*PI/180);
+ Ypos = (L2+L3)*sin((motor1Pos + motor2Pos)*PI/180) + L1*sin(motor1Pos*PI/180);
+ // check if x and y are within limits
+ // else Store the constraint line
+ // check if movement is in direction of constraint
+ // else return false no movement (anglespeed = 0)
+ // calculate required angle speeds
+ if( (Xpos>Xmax && setXSpeed > 0 )|| \
+ (Xpos<Xmin && setXSpeed < 0 )|| \
+ (Ypos>Ymax && setYSpeed > 0 )|| \
+ (Ypos<Ymin && setYSpeed < 0 ) \
+ ){
+ motor1SetSpeed = 0;
+ motor2SetSpeed = 0;
+ return false;
+ }
+ motor1SetSpeed = (180/PI)*((setXSpeed*cos((motor1Pos + motor2Pos)*PI/180) + \
+ setYSpeed*sin((motor1Pos + motor2Pos)*PI/180))/(L1*sin(motor2Pos*PI/180)));
+ motor2SetSpeed = (180/PI)*(-(setXSpeed*(L2+L3)*cos((motor1Pos + motor2Pos)*PI/180) + \
+ setYSpeed*(L2+L3)*sin((motor1Pos + motor2Pos)*PI/180) + \
+ setXSpeed*L1*cos(motor1Pos*PI/180) + \
+ setYSpeed*L1*sin(motor1Pos*PI/180))/(L1*(L2+L3)*sin(motor2Pos*PI/180)));
return true;
+}
+void runPump(){
+ pumpSwitch.write(enablePump);
}
\ No newline at end of file
--- a/actuators.h Thu Oct 29 20:06:41 2015 +0000 +++ b/actuators.h Thu Oct 29 22:46:30 2015 +0100 @@ -6,6 +6,7 @@ #include "config.h" #include "encoder.h" #include "buttons.h" +#include "serialcom.h" extern DigitalOut redLed; extern DigitalOut greenLed; @@ -36,6 +37,12 @@ // Set PID values extern PID motor2PID; +extern bool usePotmeters; +extern bool controlAngle; +extern bool controlDirection; + +extern bool enablePump; + void motorInit(); void motorControl();
--- a/debug.h Thu Oct 29 20:06:41 2015 +0000 +++ b/debug.h Thu Oct 29 22:46:30 2015 +0100 @@ -30,7 +30,7 @@ extern double x_velocity; extern double y_velocity; extern double z_velocity; -extern bool pump; +extern bool enablePump; extern int DOF; extern bool mode;
--- a/emg.cpp Thu Oct 29 20:06:41 2015 +0000
+++ b/emg.cpp Thu Oct 29 22:46:30 2015 +0100
@@ -26,7 +26,7 @@
// Initialize movement parameters
int DOF = 1; // Switch variable for controlled DOF: 1=x 2=y 3=z
-bool pump = false; // Pump switch
+bool enablePump = false; // Pump switch
bool thr_pass1 = false; // Processing threshold passed for signal 1?
bool thr_pass2 = false; // Processing threshold passed for signal 2?
double velocity = 0; // Forward velocity
@@ -165,7 +165,7 @@
}
} else {
// Switch pump ---------------------------------------------------------------------------
- pump = !pump;
+ enablePump = !enablePump;
}
}
// No input: set all value to zero ---------------------------------------------------------------
--- a/main.cpp Thu Oct 29 20:06:41 2015 +0000
+++ b/main.cpp Thu Oct 29 22:46:30 2015 +0100
@@ -8,12 +8,12 @@
*/
#include "mbed.h"
-#include "config.h" // settings and pin configurations
-#include "actuators.h"
-#include "buttons.h"
-#include "debug.h"
-#include "emg.h"
-#include "serialcom.h"
+#include "config.h" // settings and pin configurations
+#include "actuators.h" // all the actuator related functions
+#include "buttons.h" // reading switches and buttons
+#include "debug.h" // HIDscope
+#include "emg.h" // EMG
+#include "serialcom.h" // Communication with web app
Ticker switchesTick, debugTick, motorTick, EMGTick, safetyTick, serialTick;
volatile bool switches_go=false, debug_go=false, motor_go=false, emg_go=false, safety_go=false, serial_go=false;
@@ -26,73 +26,67 @@
void serial_activate(){serial_go=true;};
void tickerAttach(){
- EMGTick.attach(&emg_activate, 0.005f);
switchesTick.attach(&switches_activate, 0.02f);
- debugTick.attach(&debug_activate, motorCall);
+ // debugTick.attach(&debug_activate, motorCall);
motorTick.attach(&motor_activate, motorCall);
safetyTick.attach(&safety_activate, 0.001f);
}
double motorCall = 0.01; // set motor frequency global so it can be used for speed.
-const int sample = 0; // Constant for mode switching for program readability
-const int normalize = 1; // Constant for mode switching for program readability
-bool mode = normalize; // Set program mode
+const int sample = 0; // Constant for EMG mode switching
+const int normalize = 1; // Constant for EMG mode switching
+bool mode = normalize;
bool tickersActivated = false;
-bool calReady = false; // flag for calibration ready
+bool calReady = false; // flag for motor calibration
-bool usePotmeters = true;
-bool controlAngle = false;
-bool controlDirection = true;
+bool usePotmeters = true; // flag for using the potmeters/EMG to control the system
+bool controlAngle = false; // control the system using motor angles
+bool controlDirection = true; // control the system using kinematics
int main(){
serialInit();
serialTick.attach(&serial_activate, 0.1f); // initialze serial communication first
+ EMGTick.attach(&emg_activate, 0.005f);
motorInit();
-
-
-
while (true) {
-
- calReady = calibrateMotors();
-
- if(serial_go){
+ if(serial_go){ // communication between serial and the webpage
serial_go=false;
serialCom();
}
-
- if(calReady && !tickersActivated){ // when done with calibration, start tickers
+ if(startCalibration & !calReady){ // start calibration
+ calReady = calibrateMotors();
+ }
+ if(emg_go){
+ if(enableEMG){
+ emg_go=false;
+ readEMG();
+ }
+ }
+ if(calReady && !tickersActivated){ // when done with calibration, start rest of tickers
tickerAttach();
tickersActivated = true;
}
-
- if(emg_go){
- emg_go=false;
- readEMG();
+ if(safety_go){
+ safety_go=false;
+ safety(); // springs at the side to constrain arm movement
+ }
+ if(switches_go){
+ switches_go=false;
+ checkSwitches(); // read potmeters and buttons on the motor board
}
- if(mode==0){ // wait until EMG is done with calibration
- if(safety_go){
- safety_go=false;
- safety();
- }
- if(emg_go){
- emg_go=false;
- readEMG();
- }
- if(switches_go){
- switches_go=false;
- checkSwitches();
- }
- if(debug_go){
- debug_go=false;
- debugProcess();
- }
- if(motor_go){
- motor_go=false;
- motorControl();
- }
+ if(debug_go){
+ debug_go=false;
+ debugProcess(); // send data to HIDscope
+ }
+ if(motor_go){
+ motor_go=false;
+ motorControl(); // write data to the motors
+ }
+
+ if(mode==0){
}
}
--- a/serialcom.cpp Thu Oct 29 20:06:41 2015 +0000
+++ b/serialcom.cpp Thu Oct 29 22:46:30 2015 +0100
@@ -7,7 +7,6 @@
#include <string>
Serial pc(USBTX, USBRX);
-DigitalOut pumpSwitch(pumpPin);
char buf[128];
void serialInit(){
@@ -20,11 +19,11 @@
}
void serialCom(){
+ // Receiving
pc.scanf("%s", buf);
if(strcmp(buf,"ledRed")==0){
redLed.write(!redLed.read());
pc.printf("received data\n");
- pumpSwitch.write(!pumpSwitch.read());
}
if(strcmp(buf,"ledBlue")==0){
blueLed.write(!blueLed.read());
@@ -34,5 +33,29 @@
greenLed.write(!greenLed.read());
pc.printf("received data\n");
}
+ if(strcmp(buf, "switchPump")==0){
+ enablePump = !enablePump;
+ pc.printf("enablePump %d\n", enablePump);
+ }
+ if(strcmp(buf, "startCalibration")==0){
+ startCalibration = true;
+ pc.printf("startCalibration %d\n", startCalibration);
+ }
+ if(strcmp(buf, "enableEMG")==0){
+ enableEMG = true;
+ pc.printf("enableEMG %d\n", enableEMG);
+ }
+ if(strcmp(buf, "usePotmeters")==0){
+ usePotmeters = true;
+ pc.printf("usePotmeters %d\n", usePotmeters);
+ }
+ if(strcmp(buf, "controlAngle")==0){
+ controlAngle = true;
+ pc.printf("controlAngle %d\n", controlAngle);
+ }
+ if(strcmp(buf, "controlDirection")==0){
+ controlDirection = true;
+ pc.printf("controlDirection %d\n", controlDirection);
+ }
}
--- a/serialcom.h Thu Oct 29 20:06:41 2015 +0000 +++ b/serialcom.h Thu Oct 29 22:46:30 2015 +0100 @@ -12,6 +12,12 @@ extern DigitalOut redLed; extern DigitalOut greenLed; extern DigitalOut blueLed; +extern bool enablePump; +extern bool startCalibration; +extern bool enableEMG; +extern bool usePotmeters; +extern bool controlAngle; +extern bool controlDirection; void serialCom(); void serialInit();