Nicolas Borla
/
PES2_mbed
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: Stepper mbed Servo LSM9DS1_Library SDFileSystem
Embed:
(wiki syntax)
Show/hide line numbers
main.cpp
00001 #include "mbed.h" 00002 #include "LSM9DS1.h" 00003 #include "Servo.h" 00004 #include "EncoderCounter.h" 00005 #include "Stepper.h" 00006 #include "SDFileSystem.h" 00007 00008 DigitalOut myled(LED1); 00009 00010 // create object IMU 00011 LSM9DS1 imu(PC_9, PA_8, 0xD6, 0x3C); 00012 00013 // create servo objects 00014 Servo S0(PB_2); 00015 Servo S1(PC_8); 00016 Servo S2(PC_6); 00017 00018 DigitalOut enable(PB_15); 00019 00020 // initialise PWM 00021 PwmOut pwm_motor1(PB_13); 00022 PwmOut pwm_motor2(PA_9); 00023 PwmOut pwm_motor3(PA_10); 00024 00025 // crete Encoder read objects 00026 EncoderCounter counter1(PA_6, PC_7); // counter(pin A, pin B) 00027 EncoderCounter counter2(PB_6, PB_7); 00028 EncoderCounter counter3(PA_0, PA_1); 00029 00030 // initialise PWM 00031 Stepper stepperMotor(PB_14,PC_4); // step, dir 00032 Stepper stepperMotor2(PB_12,PA_15); // config 2 00033 00034 DigitalIn user_button(USER_BUTTON); 00035 00036 SDFileSystem sd(PC_12, PC_11, PC_10, PD_2, "sd"); // mosi miso clk cs 00037 AnalogIn ain(PC_2); 00038 DigitalOut enableStepper(PB_1); 00039 00040 // define write function 00041 void sdWrite(float a){ 00042 printf("start write SD\r\n"); 00043 00044 FILE *fp = fopen("/sd/mydir/sdtest.txt", "a"); 00045 if(fp == NULL) { 00046 error("Could not open file for write\r\n"); 00047 } 00048 fprintf(fp, "analog measure %f\r\n",a); 00049 fclose(fp); 00050 00051 printf("write succesfull\r\n\n"); 00052 } 00053 00054 int main() { 00055 // test comunication 00056 imu.begin(); 00057 if (!imu.begin()) { 00058 printf("Failed to communicate with LSM9DS1.\r\n"); 00059 } 00060 00061 // initialise and test Servo 00062 S0.Enable(1000,20000); 00063 S1.Enable(1000,20000); 00064 S2.Enable(1000,20000); 00065 00066 // initialise PWM 00067 pwm_motor1.period(0.0005f);// 0.5ms 2KHz 00068 pwm_motor1.write(0.5f); 00069 pwm_motor2.period(0.0005f);// 0.5ms 2KHz 00070 pwm_motor2.write(0.5f); 00071 pwm_motor3.period(0.0005f);// 0.5ms 2KHz 00072 pwm_motor3.write(0.5f); 00073 00074 // initialise stepper 00075 stepperMotor.setSpeed(10000); 00076 stepperMotor.rotate(stepperMotor.CW); 00077 stepperMotor.stop(); 00078 stepperMotor.setPositionZero(); 00079 00080 stepperMotor2.setSpeed(10000); 00081 stepperMotor2.rotate(stepperMotor.CW); 00082 stepperMotor2.stop(); 00083 stepperMotor2.setPositionZero(); 00084 00085 int i = 0; 00086 00087 bool buttonNow = false; 00088 bool buttonBefore = false; 00089 bool done = false; 00090 00091 enableStepper = 1; 00092 00093 00094 while(1) { 00095 00096 00097 buttonNow = !user_button; 00098 00099 if(buttonNow && !buttonBefore){i+=1; done = false;} 00100 if(i>7){i=0;} 00101 00102 switch (i){ 00103 case 1: 00104 // =================== 00105 // IMU 00106 // =================== 00107 if(!done){ 00108 printf("\r\n\r\n======================\r\nIMU test\r\n======================\r\n\r\n"); 00109 for(int i=0; i<5; i++){ 00110 // do a measure 00111 imu.readAccel(); 00112 imu.readMag(); 00113 imu.readGyro(); 00114 00115 // print values to console 00116 printf("gyro: %.5f %.5f %.5f [rad/s]\r\n", imu.gyroX,imu.gyroY,imu.gyroZ); 00117 printf("accel: %.5f %.5f %.5f [m/s^2]\r\n", imu.accX,imu.accY,imu.accZ); 00118 printf("mag: %.5f %.5f %.5f [Gauss]\r\n\n", imu.magX,imu.magY,imu.magZ); 00119 wait(0.5f); 00120 } 00121 done = true; 00122 break; 00123 } else { 00124 break; 00125 } 00126 00127 case 2: 00128 // =================== 00129 // Servo 00130 // =================== 00131 if(!done){ 00132 printf("\r\n\r\n======================\r\nServo test\r\n======================\r\n\r\n"); 00133 for(int j=0; j<2000; j+=50){ 00134 printf("position %d\r\n",j); 00135 S0.SetPosition(j); 00136 S1.SetPosition(j); 00137 S2.SetPosition(j); 00138 00139 wait(0.1f); 00140 } 00141 done = true; 00142 break; 00143 } else { 00144 break; 00145 } 00146 00147 case 3: 00148 // =================== 00149 // DC - Motor1 00150 // =================== 00151 if(!done){ 00152 printf("\r\n\r\n======================\r\nDC - Motor1 test\r\n======================\r\n\r\n"); 00153 enable = 1; 00154 for(int j=0; j<20; j+=1){ 00155 pwm_motor1.write(0.7f); 00156 pwm_motor2.write(0.7f); 00157 pwm_motor3.write(0.7f); 00158 // display encoder values to console 00159 printf("counter1 = %d counter2 = %d counter3 = %d\r\n",counter1.read(), counter2.read(), counter3.read()); 00160 wait(0.1f); 00161 } 00162 enable = 0; 00163 done = true; 00164 break; 00165 } else { 00166 break; 00167 } 00168 00169 case 4: 00170 // =================== 00171 // DC - Motor2 00172 // =================== 00173 if(!done){ 00174 printf("\r\n\r\n======================\r\nDC - Motor2 test\r\n======================\r\n\r\n"); 00175 enable = 1; 00176 for(int j=0; j<20; j+=1){ 00177 pwm_motor1.write(0.7f); 00178 pwm_motor2.write(0.7f); 00179 pwm_motor3.write(0.7f); 00180 // display encoder values to console 00181 printf("counter1 = %d counter2 = %d counter3 = %d\r\n",counter1.read(), counter2.read(), counter3.read()); 00182 wait(0.1f); 00183 } 00184 enable = 0; 00185 done = true; 00186 break; 00187 } else { 00188 break; 00189 } 00190 00191 case 5: 00192 // =================== 00193 // DC - Motor3 00194 // =================== 00195 if(!done){ 00196 printf("\r\n\r\n======================\r\nDC - Motor3 test\r\n======================\r\n\r\n"); 00197 enable = 1; 00198 for(int j=0; j<20; j+=1){ 00199 pwm_motor1.write(0.7f); 00200 pwm_motor2.write(0.7f); 00201 pwm_motor3.write(0.7f); 00202 // display encoder values to console 00203 printf("counter1 = %d counter2 = %d counter3 = %d\r\n",counter1.read(), counter2.read(), counter3.read()); 00204 wait(0.1f); 00205 } 00206 enable = 0; 00207 done = true; 00208 break; 00209 } else { 00210 break; 00211 } 00212 00213 case 6: 00214 // =================== 00215 // Stepper 00216 // =================== 00217 if(!done){ 00218 printf("\r\n\r\n======================\r\nStepper test\r\n======================\r\n\r\n"); 00219 // test stepper motor 00220 enableStepper = 0; 00221 wait(0.5f); 00222 printf("stepper enabled\r\n"); 00223 stepperMotor.goesTo(20*2048); 00224 stepperMotor2.goesTo(20*2048); 00225 wait(3.0f); 00226 stepperMotor.stop(); 00227 stepperMotor2.stop(); 00228 stepperMotor.setPositionZero(); 00229 stepperMotor2.setPositionZero(); 00230 enableStepper = 1; 00231 printf("stepper done\r\n"); 00232 done = true; 00233 break; 00234 } else { 00235 break; 00236 } 00237 00238 case 7: 00239 // =================== 00240 // SD 00241 // =================== 00242 if(!done){ 00243 printf("\r\n\r\n======================\r\nSD test\r\n======================\r\n\r\n"); 00244 00245 // crearte folder 00246 mkdir("/sd/mydir", 0777); 00247 00248 // create file 00249 printf("start write SD\r\n"); 00250 FILE *fp = fopen("/sd/mydir/sdtest.txt", "w"); 00251 if(fp == NULL) { 00252 error("Could not open file for write\r\n"); 00253 } 00254 fprintf(fp, "analog measure\r\n"); 00255 fprintf(fp, "==============\r\n"); 00256 fclose(fp); 00257 00258 // write 2 times an analog value 00259 for(int i=0; i<2; i++){ 00260 sdWrite(ain.read()); 00261 wait(2.0f); 00262 } 00263 // read from SD card 00264 printf("Reading from SD card..."); 00265 fp = fopen("/sd/mydir/sdtest.txt", "r"); 00266 if (fp != NULL) { 00267 char c = fgetc(fp); 00268 if (c == 'a') 00269 printf("success!\n"); 00270 else 00271 printf("incorrect char (%c)!\n", c); 00272 fclose(fp); 00273 } else { 00274 printf("failed!\n"); 00275 } 00276 00277 done = true; 00278 break; 00279 } else { 00280 break; 00281 } 00282 00283 00284 default: 00285 printf("\r\n\r\n======================\r\nPause\r\n======================\r\n\r\n"); 00286 break; 00287 } 00288 00289 00290 myled = 1; 00291 00292 wait(0.1); // 0.5s 00293 00294 myled = 0; 00295 } 00296 }
Generated on Thu Jul 21 2022 13:02:29 by
1.7.2