Deze moet je hebben Menner Dennis
Embed:
(wiki syntax)
Show/hide line numbers
motorDriver.cpp
00001 /* This code is written for EMILY the robot Waitress */ 00002 #include "motorDriver.h" 00003 /*============================================================================*/ 00004 // acceleration speed in ms 00005 int accSpeed = 500; 00006 int BatteryPubTime =2000; 00007 00008 00009 /*============================================================================*/ 00010 00011 00012 motorDriver::motorDriver(PinName Left1, PinName Left2, PinName Right1, PinName Right2, PinName PWML, PinName PWMR /*, PinName spdReadL, PinName spdReadR, PinName Bat*/): 00013 speedLeft1(Left1), speedLeft2(Left2), speedRight1(Right1), speedRight2(Right2) , pwmL(PWML), pwmR(PWMR)/*, speedReadL(spdReadL), speedReadR(spdReadR), BatteryRead(Bat)*/ 00014 { 00015 //initial conditions of PWM left and right 00016 pwmL.period_ms(10); 00017 pwmR.period_ms(10); 00018 00019 //initial conditions of the left motor 00020 speedLeft1= 0; 00021 speedLeft2= 0; 00022 00023 //initial conditions of the right motor 00024 speedRight1=0; 00025 speedRight2=0; 00026 00027 } 00028 00029 00030 00031 00032 void motorDriver::setDirection(bool keyForward, bool keyBackward, bool keyLeft, bool keyRight) 00033 { 00034 if(keyForward && !keyBackward && !keyLeft && !keyRight) // forward 00035 { 00036 speedLeft2 = 0; 00037 speedRight2 = 0; 00038 00039 if(speedLeft1 < 1 && speedRight1 <1) 00040 { 00041 pwmL.pulsewidth_ms(0); 00042 pwmR.pulsewidth_ms(0); 00043 speedRight1 = 1; 00044 speedLeft1 = 1; 00045 00046 for(int i=0; i<=10; i++) 00047 { 00048 pwmL.pulsewidth_ms(i); 00049 pwmR.pulsewidth_ms(i); 00050 Thread::wait(accSpeed); 00051 } 00052 00053 } 00054 00055 if(speedLeft1 && speedRight1) 00056 { 00057 speedLeft1 = 1; 00058 speedRight1 = 1; 00059 00060 } 00061 } 00062 00063 00064 if(!keyForward && keyBackward && !keyLeft && !keyRight) // Backwards 00065 { 00066 speedLeft1 = 0; 00067 speedRight1 = 0; 00068 00069 if(speedLeft2 < 1 && speedRight2 <1) 00070 { 00071 pwmL.pulsewidth_ms(0); 00072 pwmR.pulsewidth_ms(0); 00073 speedRight2 = 1; 00074 speedLeft2 = 1; 00075 00076 for(int i=0; i<=10; i++) 00077 { 00078 pwmL.pulsewidth_ms(i); 00079 pwmR.pulsewidth_ms(i); 00080 Thread::wait(accSpeed); 00081 } 00082 00083 } 00084 00085 if(speedLeft2 && speedRight2) 00086 { 00087 speedLeft1 = 1; 00088 speedRight1 = 1; 00089 Thread::wait(accSpeed); 00090 } 00091 00092 } 00093 00094 if(!keyForward && !keyBackward && keyLeft && !keyRight) // Left 00095 { 00096 speedLeft1 = 0; 00097 speedLeft2 = 0; 00098 speedRight2 = 0; 00099 00100 if(speedRight1 <1) 00101 { 00102 pwmR.pulsewidth_ms(0); 00103 speedRight1 = 1; 00104 00105 for(int i=0; i<=10; i++) 00106 { 00107 pwmR.pulsewidth_ms(i); 00108 Thread::wait(accSpeed); 00109 } 00110 00111 } 00112 00113 if(speedRight1) 00114 { 00115 speedRight1 = 1; 00116 Thread::wait(accSpeed); 00117 } 00118 } 00119 00120 if(!keyForward && !keyBackward && !keyLeft && keyRight) // Right 00121 { 00122 speedRight1 = 0; 00123 speedRight2 = 0; 00124 speedLeft2 = 0; 00125 00126 if(speedLeft1 <1) 00127 { 00128 pwmL.pulsewidth_ms(0); 00129 speedLeft1=1; 00130 00131 for(int i=0; i<=10; i++) 00132 { 00133 pwmL.pulsewidth_ms(i); 00134 Thread::wait(accSpeed); 00135 } 00136 00137 } 00138 00139 if(speedLeft1) 00140 { 00141 speedLeft1 = 1; 00142 Thread::wait(accSpeed); 00143 } 00144 } 00145 } 00146 00147 00148 // 00149 // 00150 // 00151 //void motorDriver::getBattery() 00152 //{ 00153 // 00154 // ros::Publisher pub_battery("BatteryVoltage", &value_msg); 00155 // nh.initNode(); 00156 // nh.advertise(pub_battery); 00157 // 00158 // float BatteryValue = 0; 00159 // float avg_value = 0; 00160 // 00161 // while(1) 00162 // { 00163 // for( int i = 0; i < 10; i++) 00164 // { 00165 // avg_value += BatteryRead; // uitlezen spanning 00166 // } 00167 // 00168 // BatteryValue = (avg_value / 10); // delen door 10 want 10 samples 00169 // BatteryValue = BatteryValue * 3.3; // *3.3 Vref 00170 // value_msg.data = BatteryValue; 00171 // pub_battery.publish(&value_msg); 00172 // Thread::wait(BatteryPubTime); 00173 // nh.spinOnce(); 00174 // break; 00175 // 00176 // } 00177 //}
Generated on Sun Jul 17 2022 03:22:34 by
1.7.2