Richard Roos
Total control sjoel robot
Dependencies: HIDScope MODSERIAL QEI TextLCD mbed
main.cpp
- Committer:
- RichardRoos
- Date:
- 2015-10-20
- Revision:
- 37:090ba5b1e655
- Parent:
- 36:549d1ce7b96b
File content as of revision 37:090ba5b1e655:
//--------------------Include files and libraries-------
#include "mbed.h"
#include "QEI.h"
#include "MODSERIAL.h"
#include "TextLCD.h"
#include "HIDScope.h"
#include "Filterdesigns.h"
#include "Kalibratie.h"
#include "Mode.h"
//--------------------Classes------------------------
InterruptIn btnSet(PTC6); // Kalibration button
DigitalOut ledR(LED_RED), LedB(LED3); // Leds on K64F
MODSERIAL pc(USBTX, USBRX); // Modserial voor Putty
TextLCD lcd(PTC5, PTC7, PTC0, PTC9, PTC8, PTC1); // LCD screen on inner row of K64F, rs, e, d4-d7
PwmOut servo(D3); // PwmOut servo
AnalogIn emgL(A0), emgR(A1); // Analog input of EMG, left and right
AnalogIn potL(A2), potR(A3); // Potential meter left and right
AnalogIn KS(A5); // Killswitch
HIDScope scope(6); // Hidscope, amount of scopes
Ticker EMGticker, tickerControl, tickerBreak, tickerLcd; // Ticker for EMG, regulator and break
// QEI Encoder(port 1, port 2, ,counts/rev
QEI enc1 (D13, D12, NC, 64), enc2 (D11, D10, NC, 64);
// Motor1 met PWM power control and direction
PwmOut pwmM1(D6);
DigitalOut dirM1(D7);
// Motor2 met PWM power control and direction
PwmOut pwmM2(D5);
DigitalOut dirM2(D4);
enum spelfase {CALIBRATE_EMG, CALIBRATE_AIM, CALIBRATE_PEND, AIM, BREAK, FIRE}; // Proframstates, ACKNOWLEDGEMENT switch: BMT groep 4 2014
uint8_t state = CALIBRATE_EMG; // first state program
enum aimFase {OFF, CW, CCW}; // Aim motor possible states
uint8_t aimState = OFF; // first state aim motor
//-------------------------------Variables---------------------------------------------------------------------
const int on = 0, off = 1; // on off
int maxCounts = 13000; // max richt motor counts Aim motor
int breakPerc = 0;
const double servoL = 0.001, servoR = 0.0011; // Range servo,between servoL en servoR (= pulsewidth pwm servo)
const double tControl = 0.005, tBreak = 0.1, tLcd = 2; // tickers
const double Fs = 50; //Sample frequency Hz
double t = 1/ Fs; // time EMGticker
double thresholdlowL= 0, thresholdmidL = 0, thresholdhighL= 0;
double thresholdlowR= 0, thresholdmidR = 0, thresholdhighR= 0;
double yL = 0, yR = 0; // y values EMG left and right
volatile bool L = false, R = false; // Booleans for checking if mode. has been 1?
volatile bool btn = false; // Button is pressed?
volatile bool controlFlag = false, btnFlag = false, breakFlag = false, emgFlag = false, lcdFlag = false; // Go flags
//----------------------------Functions-----------------------------------------------------------------------
void flipLed(DigitalOut& led){ //function to flip one LED
led.write(on);
wait(0.2);
led.write(off);
}
void PRINT(const char* text){
lcd.cls(); // clear LCD scherm
lcd.printf(text); // print text op lcd
}
void EMGkalibratieL(){ // Determine thresholds left
PRINT("EMG LEFT relax muscle");
double ymin = KalibratieMin(emgL, true); // Minimum value left EMG, boolean indicates left
wait(1);
PRINT("EMG LEFT flex muscle"); // LCD
double ymax = KalibratieMax(emgL, true); // Maxium value left EMG, boolean indicates left
PRINT("EMG LEFT well done!"); // LCD
if((ymax-ymin) < 0.05){ // No EMG connected
ymin = 10;
ymax = 10;
}
thresholdlowL = 4 * ymin; // Lowest threshold
thresholdmidL = 0.5 * ymax; // Midi threshold
thresholdhighL = 0.8 * ymax; // Highest threshold
pc.printf("ymaxL = %f en yminL = %f \r\n",ymax, ymin);
}
void EMGkalibratieR(){ // Determine thresholds right EMG, same as left
PRINT("EMG RIGHT relax muscle");
double ymin = KalibratieMin(emgR, false);
wait(1);
PRINT("EMG RIGHT flex muscle");
double ymax = KalibratieMax(emgR, false);
PRINT("EMG LEFT well done!");
if((ymax-ymin) < 0.05){
ymin = 10;
ymax = 10;
}
thresholdlowR = 4 * ymin;
thresholdmidR = 0.5 * ymax;
thresholdhighR = 0.8 * ymax;
pc.printf("ymaxR = %f en yminR = %f \r\n",ymax, ymin); // terminal
}
int EMGfilter(AnalogIn& emg, bool side){
double u = emg.read(); // read emg signal (left or right EMG)
int mode = 1;
if(side){
double y = FilterdesignsLeft(u); // filter signal left EMG
mode = Mode(y, thresholdlowL, thresholdmidL, thresholdhighL); // Determine mode with thresholds (1, 2, 3)
}
else {
double y = FilterdesignsRight(u);
mode = Mode(y, thresholdlowR, thresholdmidR, thresholdhighR); // right EMG
}
return mode;
}
int PotReader(AnalogIn& pot){ // read potentialmeter and determine its mode (1 = default, 2, 3)
double volt = pot.read();
int mode = 1;
if(volt > 0.8){
mode = 3;
}
else if(volt>0.35 && volt<0.65){
mode = 2;
}
return mode;
}
int defMode(AnalogIn& emg, AnalogIn& pot, bool side){ // Determine mode both from EMG and Potentialmeter, ONE of them must be ONE!
int emgMode = EMGfilter(emg, side);
int potMode = PotReader(pot);
int mode = 1;
if(!(emgMode==1) != !(potMode==1)){ // emgMode = 1 XOR potMode = 1
if(emgMode > potMode){ // maximum of emg and pot
mode = emgMode;
}
else{
mode = potMode;
}
}
return mode;
}
void setEmgFlag(){ // Goflag EMG
emgFlag = true;
}
void setLcdFlag(){ // Goflag EMG
lcdFlag = true;
}
void btnSetAction(){ // Set knop K64F
btn = true; // GoFlag setknop
}
void setControlFlag(){ // Go flag setButton
controlFlag = true;
}
void setBreakFlag(){ // Go flag Break
breakFlag = true;
}
void checkAim(){ // check if Killswitch is on or max counts is reached
double volt = KS.read();
if(volt> 0.5 || abs(enc2.getPulses()) > (maxCounts -50) || enc2.getPulses() < 0){
pwmM2.write(0); // Aim motor freeze
pc.printf("BOEM! CRASH! KASTUK! \r\n"); // Terminal
PRINT("BOEM! CRASH!"); // LCD
}
}
void motorAim(int dir){ // Rotate Aim motor with given direction
dirM2.write(dir);
pwmM2.write(0.25);
}
bool controlAim(){ // Function to control aim motor with modes
bool both = false; // boolean if both modes are 3
int modeL = defMode(emgL, potL, true);
int modeR = defMode(emgR, potR, false);
scope.set(0, modeL);
scope.set(1, modeR);
scope.send(); //send values to HIDScope
if(modeR < 2 && !R){ // control if mode has been 1
R = true;
}
if(modeL < 2 && !L){
L = true;
}
if((modeL>2) && (modeR >2 && R && L)) { // mode L and mode R both 3, and both has been 1 herefore
both = true; // Return true
pwmM2.write(0); // Aim motor freeze
aimState = OFF; // next state
}
else if((modeR == 2) && (modeL == 2)) { // modes are both 2
if(aimState!=OFF){ // only if aim motor is rotating
pwmM2.write(0); // Aim motor freeze
aimState = OFF; // motor state is off
pc.printf("Motor freeze\r\n"); // LCD
L = false; // Modes must be first 1 for another action
R = false; // ""
}
}
else if((modeL == 2) && (aimState != CCW && (modeR == 1))) { // modeL ==2 AND rotation is not CCW AND modeR has been 1
if(L){
aimState = CCW; // Rotate CCW
pc.printf("Rotate -, CCW\r\n");
motorAim(0);
}
}
else if((modeR == 2) && (aimState != CW && (modeL == 1))) { // modeR == 2 AND rotation is not CW AND modeL has been 1
if(R){
aimState = CW; // Rotate CW
pc.printf("Rotate +, CW\r\n");
motorAim(1);
}
}
return both;
}
int main(){
flipLed(ledR); // test if code begins
btnSet.mode(PullUp); // Button mode
btnSet.rise(&btnSetAction); // Connect button to function
tickerControl.attach(&setControlFlag,tControl); // ticker control motor
tickerBreak.attach(&setBreakFlag,tBreak); // ticker break
EMGticker.attach(&setEmgFlag, t); // ticker EMG, 500H
tickerLcd.attach(&setLcdFlag,tLcd); // ticker lcd
pc.printf("\n\n\n\n\n"); // Terminal
pc.printf("---NEW GAME---\r\n");
PRINT("--- NEW GAME ---"); // LCD
while(1){ // Run program
switch(state){
case CALIBRATE_EMG: {
EMGkalibratieL(); // calibrate left EMG, determine thresholds
EMGkalibratieR(); // calibrate right EMG, determine thresholds
state = CALIBRATE_AIM; // Next state
break;
}
case CALIBRATE_AIM: {
pwmM2.period(1/100000); // period motor 2
printf("Position is kalibrating\r\n"); // terminal
PRINT("Wait a moment, please"); // LCD
dirM2.write(0); // direction aim motor
pwmM2.write(0.1); // percentage motor power
bool calibrated = false; //
while(state==CALIBRATE_AIM){
pc.printf("Killswitch: %f\r\n", KS.read()); // terminal
if(KS.read() > 0.5){ // Killswitch? //2* gedaan, zodat breuk uit de if-statement is
pwmM2.write(0); // Aim motor freeze
enc2.reset(); // Reset encoder
PRINT("Aim calibrated"); // LCD
calibrated = true; //
state = CALIBRATE_PEND; // next state
//dirM2.write(1); // direction aim motor
//pwmM2.write(0.25); // percentage motor power, turn it on
}
/*
if(controlFlag && calibrated){ // motor regelen op GoFlag
controlFlag = false;
if(enc1.getPulses() > maxCounts/2){ // rotate aim motor to midi position
pwmM2.write(0); // Aim motor freeze
state = CALIBRATE_PEND; // next state
}
} */
}
break;
}
case CALIBRATE_PEND: {
pc.printf("Calibrate beam motor with setknop\r\n"); // Terminal
pwmM1.period(1/100000); // period beam motor
servo.period(0.02); // 20ms period servo
//pwmM1.write(0.25); // Turn motor on, low power
btn = false; // Button is unpressed
R = false; // modeR must become 1
L = false; // modeL must become 1
PRINT("Calibrate beam to 10 o'clock");
wait(1);
PRINT("Flex right half to swing beam");
while(state==CALIBRATE_PEND){
if(emgFlag){
pc.printf(""); // lege regel printen, anders doet setknop het niet
emgFlag = false;
int modeL = defMode(emgL, potL, true); // determine modeL
int modeR = defMode(emgR, potR, false); // determine modeR
if(modeR < 2) { // modeR == 1
R = true;
}
if(modeL < 2) { // modeL == 1
L = true;
}
if (btn || (modeL == 3 && L) || (modeR == 3 && R)){ // If setbutton is on or one mode is 3, and has been 1
pwmM1.write(0); // Motor 1 freeze
enc1.reset(); // encoder 1 reset
PRINT("Beam calibrated");
btn = false; // button op false
state = AIM; // next state
}
else if(modeL == 2){
pwmM1.write(0); // beam freeze
PRINT("Flex both full to continue"); // LCD
}
else if(modeR == 2){
pwmM1.write(0.025); // beam rotate
PRINT("Flex left half to stop beam"); // LCD
}
}
}
lcd.cls();
break;
}
case AIM: {
pc.printf("Aim with EMG\r\n"); // terminal
int i = 0; // counter for lcd
while(state == AIM){
if(lcdFlag){ // LCD loopje to project 3 texts on lcd
lcdFlag = false;
if(i%3 == 0){
PRINT("Flex left or right half to aim");
}
else if(i%3 == 1){
PRINT("Flex both half to freeze");
}
else {
PRINT("Flex both full to continue");
}
i++;
}
if(controlFlag){ // motor control on GoFlag
controlFlag = false;
checkAim(); // Check position
}
if(emgFlag){ // Go flag EMG sampel
emgFlag = false;
if(controlAim()){
pc.printf("Position chosen\r\n"); // terminal
state = BREAK; // Next state (BREAK)
}
}
}
lcd.cls();
break;
}
case BREAK: {
pc.printf("Set break percentage, current is: %.0f\r\n", breakPerc);
L = false;
R = false;
int i = 0; // counter for lcd
while(state == BREAK){
if(lcdFlag){ // LCD loop to project text on LCD
lcdFlag = false;
if(i%2 == 0){
PRINT("Flex L or R to adjust break");
}
else {
PRINT("Flex both to continue");
}
i++;
}
if(emgFlag){ // Go flag EMG sampelen
emgFlag = false;
int modeL = defMode(emgL, potL, true);
int modeR = defMode(emgR, potR, false);
if(modeR < 2) { // modeR is 1
R = true;
}
if(modeL < 2) { // modeL is 1
L = true;
}
if(L && R){
if((modeL > 1) && modeR > 1) { // both modes
state = FIRE;
R = false;
L = false;
}
else if(modeL > 2 || modeR > 2) { // left of right mode 3 = fire
state = FIRE;
R = false;
L = false;
}
else if((modeL == 2) && L) { // modeL = BREAK lower with one
if(breakPerc>1){
breakPerc--;
}
L = false;
}
else if((modeR == 2) && R) { // modeR = Break +1
if(breakPerc<10){
breakPerc++;
}
R = false;
}
if(modeL > 1 || modeR > 1){ // Print BREAK scale if one of both modes > 1
pc.printf("Current breaking scale: %i\r\n", breakPerc);
lcd.cls();
lcd.printf("Break scale: %i", breakPerc);
}
}
}
}
L = false; // modeL must be one for another action can take place
R = false; // modeR ""
lcd.cls();
break;
}
case FIRE: {
pc.printf("Shooting\r\n");
PRINT("FIRING");
servo.pulsewidth(servoL); // BREAK release
pwmM1.write(0.25); // beam motor on
bool servoPos = true;
while(state == FIRE){ // while in FIRE state
if(breakFlag && (abs(enc1.getPulses())-100)%4200 < (2100*abs(breakPerc)/10)){ // BREAK goFlag half rotation beam = breaking
breakFlag = false;
if(servoPos){
servoPos = false;
servo.pulsewidth(servoL); // left
}
else{
servoPos = true;
servo.pulsewidth(servoR); // right
}
}
if(controlFlag){
controlFlag = false;
if((abs(enc1.getPulses())+50)%4200 < 150){ // rotate beam one revolution
pwmM1.write(0); // motor beam off
pc.printf("Beam on startposition\r\n");
state = AIM; // Next stage
}
}
}
lcd.cls();
break;
}
}
}
}