main.cpp

00001 #include "mbed.h"
00002 #include "MODSERIAL.h"
00003 #include "QEI.h"
00004 
00005 
00006 DigitalOut M1_Rotate(D2); // voltage only base rotation
00007 PwmOut M1_Speed(D3);      // voltage only base rotation
00008 
00009 MODSERIAL pc(USBTX, USBRX);
00010 
00011 //QEI wheel(PinName channelA, PinName channelB, PinName index, int pulsesPerRev, Encoding encoding=X2_ENCODING)
00012 QEI motor2(D10,D11,NC,8400,QEI::X4_ENCODING);
00013 QEI motor3(D12,D13,NC,8400,QEI::X4_ENCODING);
00014 
00015 DigitalOut M2_Rotate(D4);   // encoder side pot 2 translation
00016 PwmOut M2_Speed(D5);        // encoder side pot 2 translation
00017 
00018 DigitalOut M3_Rotate(D7);   // encoder side pot 1 spatel rotation
00019 PwmOut M3_Speed(D6);        // encoder side pot 1 spatel rotation
00020 
00021 DigitalIn links(SW3);
00022 DigitalIn rechts(SW2);
00023 //DigitalOut M2_rotate(D6);
00024 //PwmOut M2_Speed(D7);
00025 
00026 AnalogIn pot1(A4); // pot 1 motor 1
00027 AnalogIn pot2(A3); // pot 2 motor 3
00028 
00029 bool draairechts;
00030 bool draailinks;
00031 bool turn = 0;
00032 float waiter = 0.1;
00033 float afstand = 50;
00034 float translation = 0;
00035 float degrees3 = 0;
00036 
00037 float Puls_degree = (8400/360);
00038 float wheel1 = 16;
00039 float wheel2 = 31;
00040 float wheel3 = 41;
00041 float overbrenging = ((wheel2/wheel1)*(wheel3/wheel1));
00042 float pi = 3.14159265359;
00043 
00044 void GetDirections()
00045 {
00046     pc.baud(115200);
00047     if ((rechts == 0) && (links == 0) && (turn == 0)) {
00048         draailinks = 0;
00049         draairechts = 0;
00050         turn = 1;
00051         pc.printf("begin de actie \n \r ");
00052         wait(waiter);
00053 
00054     } else if ((rechts == 0) && (links == 0) && (turn == 1)) {
00055         draailinks = 0;
00056         draairechts = 0;
00057         turn = 0;
00058         pc.printf("breek de actie af \n \r ");
00059         wait(waiter);
00060     } else if ((rechts == 1) && (links == 1)&& (turn == 0)) {
00061 
00062     } else if ((rechts == 1) && (draailinks == 0)&& (turn == 0)) {
00063         /* if the right button is pressed and the motor isn't rotating to the left,
00064         then start rotating to the right etc*/
00065         draairechts = !draairechts;
00066         pc.printf("draai naar rechts \n \r ");
00067         wait(waiter);
00068     } else if ((rechts == 1) && (draailinks == 1)&& (turn == 0)) {
00069         draailinks = 0;
00070         draairechts = !draairechts;
00071         pc.printf("draai naar rechts na links \n \r ");
00072         wait(waiter);
00073     } else if ((links == 1) && (draairechts == 0)&& (turn == 0)) {
00074         draailinks = !draailinks;
00075         pc.printf("draai naar links \n \r ");
00076         wait(waiter);
00077     } else if ((links == 1) && (draairechts == 1) && (turn == 0)) {
00078         draairechts = 0;
00079         draailinks = !draailinks;
00080         pc.printf("draai naar links na rechts \n \r ");
00081         wait(waiter);
00082     }
00083     wait(2*waiter);
00084 }
00085 
00086 float GetPositionM2()
00087 {
00088     float pulses2 = motor2.getPulses();
00089     float degrees2 = (pulses2/Puls_degree);
00090     float radians2 = (degrees2/360)*2*pi;
00091     float translation = ((radians2/overbrenging)*32.25);
00092 
00093     return translation;
00094 }
00095 float GetRotationM3()
00096 {
00097     float pulses3 = motor3.getPulses();
00098     float degrees3 = (pulses3/Puls_degree);
00099     float radians3 = (degrees3/360)*2*pi;
00100 
00101     return degrees3;
00102 }
00103 void GoBack()
00104 {
00105     while (GetPositionM2() > 0) {
00106 
00107         M2_Speed = 0.8;
00108         M2_Rotate = 0;
00109         pc.printf("rotation %f translation %f \n \r ", GetRotationM3(), GetPositionM2());
00110     }
00111     M2_Speed = 0;
00112 
00113 
00114     while (GetRotationM3 < 0) {
00115         M3_Rotate = 1;
00116         M3_Speed = 0.2;
00117         
00118     }
00119     M3_Speed = 0;
00120 
00121     turn = 0;
00122     
00123 }
00124 void Burgerflip()
00125 {
00126     if (GetPositionM2() > afstand) {
00127         M3_Speed = 0.2;
00128         M3_Rotate = 0;
00129         M2_Speed = 0;
00130     } else if (GetPositionM2() < afstand) {
00131         M2_Speed = 0.8;
00132         M2_Rotate = 1;
00133     }
00134     if (GetRotationM3() > 75) {
00135         GoBack();
00136     }
00137 }
00138 
00139 int main()
00140 {
00141     while (true) {
00142         
00143         GetDirections();
00144         if (draairechts == true) {
00145             M1_Speed = 0.2;
00146             M1_Rotate = 0;
00147         } else if (draailinks == true) {
00148             M1_Speed = 0.2;
00149             M1_Rotate = 1;
00150         } else if (turn == 1) {
00151             /*M2_Speed = 0.5;
00152             M2_Rotate = 1;
00153             M3_Speed = 0.5;
00154             M3_Rotate = 1;*/
00155             Burgerflip();
00156         } else if (turn == 0) {
00157             M2_Speed = 0;
00158             M3_Speed = 0;
00159         }
00160         if ((draailinks == false) && (draairechts == false)) {
00161             M1_Speed = 0;
00162         }
00163         pc.printf("rotation %f translation %f \n \r ", GetRotationM3(), GetPositionM2());
00164         /* pulses = 8400 */
00165     }
00166 }
00167 /*
00168         float GetReferenceVelocity() {
00169             // Returns reference velocity in rad/s.
00170             // Positive value means clockwise rotation.
00171             const float maxVelocity=8.4; // in rad/s of course!    float referenceVelocity;  // in rad/s
00172             if (a)   {
00173                 // Clockwise rotation
00174                 referenceVelocity = potMeterIn * maxVelocity;
00175                 
00176             } else {
00177                 // Counterclockwise rotation
00178                 referenceVelocity = -1*potMeterIn * maxVelocity;
00179                 
00180             }
00181             return referenceVelocity;
00182         }
00183 
00184         void SetMotor1(float motorValue) {
00185             // Given -1<=motorValue<=1, this sets the PWM and direction
00186             // bits for motor 1. Positive value makes motor rotating
00187             // clockwise. motorValues outside range are truncated to
00188             // within range
00189             if (motorValue >=0) {
00190                 motor1DirectionPin=1;
00191             } else {
00192                 motor1DirectionPin=0;
00193             }
00194             if (fabs(motorValue)>1) {
00195                 motor1MagnitudePin = 1;
00196             } else {
00197                 motor1MagnitudePin = fabs(motorValue);
00198             }
00199         }
00200 
00201         void SetMotor2(float motorValue) {
00202             // Given -1<=motorValue<=1, this sets the PWM and direction
00203             // bits for motor 1. Positive value makes motor rotating
00204             // clockwise. motorValues outside range are truncated to
00205             // within range
00206             if (motorValue >=0) {
00207                 motor2DirectionPin=1;
00208             } else {
00209                 motor2DirectionPin=0;
00210             }
00211             if (fabs(motorValue)>1) {
00212                 motor2MagnitudePin = 1;
00213             } else {
00214                 motor2MagnitudePin = fabs(motorValue);
00215             }
00216         }
00217 
00218         void SetMotor3(float motorValue) {
00219             // Given -1<=motorValue<=1, this sets the PWM and direction
00220             // bits for motor 1. Positive value makes motor rotating
00221             // clockwise. motorValues outside range are truncated to
00222             // within range
00223             if (motorValue >=0) {
00224                 motor3DirectionPin=1;
00225             } else {
00226                 motor3DirectionPin=0;
00227             }
00228             if (fabs(motorValue)>1) {
00229                 motor3MagnitudePin = 1;
00230             } else {
00231                 motor3MagnitudePin = fabs(motorValue);
00232             }
00233         }
00234         */