Nicolas Borla / Mbed 2 deprecated PES2_mbed

PES2 Firmware for mbed2

Dependencies:   Stepper mbed Servo LSM9DS1_Library SDFileSystem

main.cpp@0:41afd907b7bd, 2021-03-12 (annotated)

Committer:
boro
Date:
Fri Mar 12 13:06:52 2021 +0000
Revision:
0:41afd907b7bd
d

Who changed what in which revision?

UserRevisionLine numberNew contents of line
boro 0:41afd907b7bd 1 #include "mbed.h"
boro 0:41afd907b7bd 2 #include "LSM9DS1.h"
boro 0:41afd907b7bd 3 #include "Servo.h"
boro 0:41afd907b7bd 4 #include "EncoderCounter.h"
boro 0:41afd907b7bd 5 #include "Stepper.h"
boro 0:41afd907b7bd 6 #include "SDFileSystem.h"
boro 0:41afd907b7bd 7
boro 0:41afd907b7bd 8 DigitalOut myled(LED1);
boro 0:41afd907b7bd 9
boro 0:41afd907b7bd 10 // create object IMU
boro 0:41afd907b7bd 11 LSM9DS1 imu(PC_9, PA_8, 0xD6, 0x3C);
boro 0:41afd907b7bd 12
boro 0:41afd907b7bd 13 // create servo objects
boro 0:41afd907b7bd 14 Servo S0(PB_2);
boro 0:41afd907b7bd 15 Servo S1(PC_8);
boro 0:41afd907b7bd 16 Servo S2(PC_6);
boro 0:41afd907b7bd 17
boro 0:41afd907b7bd 18 DigitalOut enable(PB_15);
boro 0:41afd907b7bd 19
boro 0:41afd907b7bd 20 // initialise PWM
boro 0:41afd907b7bd 21 PwmOut pwm_motor1(PB_13);
boro 0:41afd907b7bd 22 PwmOut pwm_motor2(PA_9);
boro 0:41afd907b7bd 23 PwmOut pwm_motor3(PA_10);
boro 0:41afd907b7bd 24
boro 0:41afd907b7bd 25 // crete Encoder read objects
boro 0:41afd907b7bd 26 EncoderCounter counter1(PA_6, PC_7); // counter(pin A, pin B)
boro 0:41afd907b7bd 27 EncoderCounter counter2(PB_6, PB_7);
boro 0:41afd907b7bd 28 EncoderCounter counter3(PA_0, PA_1);
boro 0:41afd907b7bd 29
boro 0:41afd907b7bd 30 // initialise PWM
boro 0:41afd907b7bd 31 Stepper stepperMotor(PB_14,PC_4); // step, dir
boro 0:41afd907b7bd 32 Stepper stepperMotor2(PB_12,PA_15); // config 2
boro 0:41afd907b7bd 33
boro 0:41afd907b7bd 34 DigitalIn user_button(USER_BUTTON);
boro 0:41afd907b7bd 35
boro 0:41afd907b7bd 36 SDFileSystem sd(PC_12, PC_11, PC_10, PD_2, "sd"); // mosi miso clk cs
boro 0:41afd907b7bd 37 AnalogIn ain(PC_2);
boro 0:41afd907b7bd 38 DigitalOut enableStepper(PB_1);
boro 0:41afd907b7bd 39
boro 0:41afd907b7bd 40 // define write function
boro 0:41afd907b7bd 41 void sdWrite(float a){
boro 0:41afd907b7bd 42 printf("start write SD\r\n");
boro 0:41afd907b7bd 43
boro 0:41afd907b7bd 44 FILE *fp = fopen("/sd/mydir/sdtest.txt", "a");
boro 0:41afd907b7bd 45 if(fp == NULL) {
boro 0:41afd907b7bd 46 error("Could not open file for write\r\n");
boro 0:41afd907b7bd 47 }
boro 0:41afd907b7bd 48 fprintf(fp, "analog measure %f\r\n",a);
boro 0:41afd907b7bd 49 fclose(fp);
boro 0:41afd907b7bd 50
boro 0:41afd907b7bd 51 printf("write succesfull\r\n\n");
boro 0:41afd907b7bd 52 }
boro 0:41afd907b7bd 53
boro 0:41afd907b7bd 54 int main() {
boro 0:41afd907b7bd 55 // test comunication
boro 0:41afd907b7bd 56 imu.begin();
boro 0:41afd907b7bd 57 if (!imu.begin()) {
boro 0:41afd907b7bd 58 printf("Failed to communicate with LSM9DS1.\r\n");
boro 0:41afd907b7bd 59 }
boro 0:41afd907b7bd 60
boro 0:41afd907b7bd 61 // initialise and test Servo
boro 0:41afd907b7bd 62 S0.Enable(1000,20000);
boro 0:41afd907b7bd 63 S1.Enable(1000,20000);
boro 0:41afd907b7bd 64 S2.Enable(1000,20000);
boro 0:41afd907b7bd 65
boro 0:41afd907b7bd 66 // initialise PWM
boro 0:41afd907b7bd 67 pwm_motor1.period(0.0005f);// 0.5ms 2KHz
boro 0:41afd907b7bd 68 pwm_motor1.write(0.5f);
boro 0:41afd907b7bd 69 pwm_motor2.period(0.0005f);// 0.5ms 2KHz
boro 0:41afd907b7bd 70 pwm_motor2.write(0.5f);
boro 0:41afd907b7bd 71 pwm_motor3.period(0.0005f);// 0.5ms 2KHz
boro 0:41afd907b7bd 72 pwm_motor3.write(0.5f);
boro 0:41afd907b7bd 73
boro 0:41afd907b7bd 74 // initialise stepper
boro 0:41afd907b7bd 75 stepperMotor.setSpeed(10000);
boro 0:41afd907b7bd 76 stepperMotor.rotate(stepperMotor.CW);
boro 0:41afd907b7bd 77 stepperMotor.stop();
boro 0:41afd907b7bd 78 stepperMotor.setPositionZero();
boro 0:41afd907b7bd 79
boro 0:41afd907b7bd 80 stepperMotor2.setSpeed(10000);
boro 0:41afd907b7bd 81 stepperMotor2.rotate(stepperMotor.CW);
boro 0:41afd907b7bd 82 stepperMotor2.stop();
boro 0:41afd907b7bd 83 stepperMotor2.setPositionZero();
boro 0:41afd907b7bd 84
boro 0:41afd907b7bd 85 int i = 0;
boro 0:41afd907b7bd 86
boro 0:41afd907b7bd 87 bool buttonNow = false;
boro 0:41afd907b7bd 88 bool buttonBefore = false;
boro 0:41afd907b7bd 89 bool done = false;
boro 0:41afd907b7bd 90
boro 0:41afd907b7bd 91 enableStepper = 1;
boro 0:41afd907b7bd 92
boro 0:41afd907b7bd 93
boro 0:41afd907b7bd 94 while(1) {
boro 0:41afd907b7bd 95
boro 0:41afd907b7bd 96
boro 0:41afd907b7bd 97 buttonNow = !user_button;
boro 0:41afd907b7bd 98
boro 0:41afd907b7bd 99 if(buttonNow && !buttonBefore){i+=1; done = false;}
boro 0:41afd907b7bd 100 if(i>7){i=0;}
boro 0:41afd907b7bd 101
boro 0:41afd907b7bd 102 switch (i){
boro 0:41afd907b7bd 103 case 1:
boro 0:41afd907b7bd 104 // ===================
boro 0:41afd907b7bd 105 // IMU
boro 0:41afd907b7bd 106 // ===================
boro 0:41afd907b7bd 107 if(!done){
boro 0:41afd907b7bd 108 printf("\r\n\r\n======================\r\nIMU test\r\n======================\r\n\r\n");
boro 0:41afd907b7bd 109 for(int i=0; i<5; i++){
boro 0:41afd907b7bd 110 // do a measure
boro 0:41afd907b7bd 111 imu.readAccel();
boro 0:41afd907b7bd 112 imu.readMag();
boro 0:41afd907b7bd 113 imu.readGyro();
boro 0:41afd907b7bd 114
boro 0:41afd907b7bd 115 // print values to console
boro 0:41afd907b7bd 116 printf("gyro: %.5f %.5f %.5f [rad/s]\r\n", imu.gyroX,imu.gyroY,imu.gyroZ);
boro 0:41afd907b7bd 117 printf("accel: %.5f %.5f %.5f [m/s^2]\r\n", imu.accX,imu.accY,imu.accZ);
boro 0:41afd907b7bd 118 printf("mag: %.5f %.5f %.5f [Gauss]\r\n\n", imu.magX,imu.magY,imu.magZ);
boro 0:41afd907b7bd 119 wait(0.5f);
boro 0:41afd907b7bd 120 }
boro 0:41afd907b7bd 121 done = true;
boro 0:41afd907b7bd 122 break;
boro 0:41afd907b7bd 123 } else {
boro 0:41afd907b7bd 124 break;
boro 0:41afd907b7bd 125 }
boro 0:41afd907b7bd 126
boro 0:41afd907b7bd 127 case 2:
boro 0:41afd907b7bd 128 // ===================
boro 0:41afd907b7bd 129 // Servo
boro 0:41afd907b7bd 130 // ===================
boro 0:41afd907b7bd 131 if(!done){
boro 0:41afd907b7bd 132 printf("\r\n\r\n======================\r\nServo test\r\n======================\r\n\r\n");
boro 0:41afd907b7bd 133 for(int j=0; j<2000; j+=50){
boro 0:41afd907b7bd 134 printf("position %d\r\n",j);
boro 0:41afd907b7bd 135 S0.SetPosition(j);
boro 0:41afd907b7bd 136 S1.SetPosition(j);
boro 0:41afd907b7bd 137 S2.SetPosition(j);
boro 0:41afd907b7bd 138
boro 0:41afd907b7bd 139 wait(0.1f);
boro 0:41afd907b7bd 140 }
boro 0:41afd907b7bd 141 done = true;
boro 0:41afd907b7bd 142 break;
boro 0:41afd907b7bd 143 } else {
boro 0:41afd907b7bd 144 break;
boro 0:41afd907b7bd 145 }
boro 0:41afd907b7bd 146
boro 0:41afd907b7bd 147 case 3:
boro 0:41afd907b7bd 148 // ===================
boro 0:41afd907b7bd 149 // DC - Motor1
boro 0:41afd907b7bd 150 // ===================
boro 0:41afd907b7bd 151 if(!done){
boro 0:41afd907b7bd 152 printf("\r\n\r\n======================\r\nDC - Motor1 test\r\n======================\r\n\r\n");
boro 0:41afd907b7bd 153 enable = 1;
boro 0:41afd907b7bd 154 for(int j=0; j<20; j+=1){
boro 0:41afd907b7bd 155 pwm_motor1.write(0.7f);
boro 0:41afd907b7bd 156 pwm_motor2.write(0.7f);
boro 0:41afd907b7bd 157 pwm_motor3.write(0.7f);
boro 0:41afd907b7bd 158 // display encoder values to console
boro 0:41afd907b7bd 159 printf("counter1 = %d counter2 = %d counter3 = %d\r\n",counter1.read(), counter2.read(), counter3.read());
boro 0:41afd907b7bd 160 wait(0.1f);
boro 0:41afd907b7bd 161 }
boro 0:41afd907b7bd 162 enable = 0;
boro 0:41afd907b7bd 163 done = true;
boro 0:41afd907b7bd 164 break;
boro 0:41afd907b7bd 165 } else {
boro 0:41afd907b7bd 166 break;
boro 0:41afd907b7bd 167 }
boro 0:41afd907b7bd 168
boro 0:41afd907b7bd 169 case 4:
boro 0:41afd907b7bd 170 // ===================
boro 0:41afd907b7bd 171 // DC - Motor2
boro 0:41afd907b7bd 172 // ===================
boro 0:41afd907b7bd 173 if(!done){
boro 0:41afd907b7bd 174 printf("\r\n\r\n======================\r\nDC - Motor2 test\r\n======================\r\n\r\n");
boro 0:41afd907b7bd 175 enable = 1;
boro 0:41afd907b7bd 176 for(int j=0; j<20; j+=1){
boro 0:41afd907b7bd 177 pwm_motor1.write(0.7f);
boro 0:41afd907b7bd 178 pwm_motor2.write(0.7f);
boro 0:41afd907b7bd 179 pwm_motor3.write(0.7f);
boro 0:41afd907b7bd 180 // display encoder values to console
boro 0:41afd907b7bd 181 printf("counter1 = %d counter2 = %d counter3 = %d\r\n",counter1.read(), counter2.read(), counter3.read());
boro 0:41afd907b7bd 182 wait(0.1f);
boro 0:41afd907b7bd 183 }
boro 0:41afd907b7bd 184 enable = 0;
boro 0:41afd907b7bd 185 done = true;
boro 0:41afd907b7bd 186 break;
boro 0:41afd907b7bd 187 } else {
boro 0:41afd907b7bd 188 break;
boro 0:41afd907b7bd 189 }
boro 0:41afd907b7bd 190
boro 0:41afd907b7bd 191 case 5:
boro 0:41afd907b7bd 192 // ===================
boro 0:41afd907b7bd 193 // DC - Motor3
boro 0:41afd907b7bd 194 // ===================
boro 0:41afd907b7bd 195 if(!done){
boro 0:41afd907b7bd 196 printf("\r\n\r\n======================\r\nDC - Motor3 test\r\n======================\r\n\r\n");
boro 0:41afd907b7bd 197 enable = 1;
boro 0:41afd907b7bd 198 for(int j=0; j<20; j+=1){
boro 0:41afd907b7bd 199 pwm_motor1.write(0.7f);
boro 0:41afd907b7bd 200 pwm_motor2.write(0.7f);
boro 0:41afd907b7bd 201 pwm_motor3.write(0.7f);
boro 0:41afd907b7bd 202 // display encoder values to console
boro 0:41afd907b7bd 203 printf("counter1 = %d counter2 = %d counter3 = %d\r\n",counter1.read(), counter2.read(), counter3.read());
boro 0:41afd907b7bd 204 wait(0.1f);
boro 0:41afd907b7bd 205 }
boro 0:41afd907b7bd 206 enable = 0;
boro 0:41afd907b7bd 207 done = true;
boro 0:41afd907b7bd 208 break;
boro 0:41afd907b7bd 209 } else {
boro 0:41afd907b7bd 210 break;
boro 0:41afd907b7bd 211 }
boro 0:41afd907b7bd 212
boro 0:41afd907b7bd 213 case 6:
boro 0:41afd907b7bd 214 // ===================
boro 0:41afd907b7bd 215 // Stepper
boro 0:41afd907b7bd 216 // ===================
boro 0:41afd907b7bd 217 if(!done){
boro 0:41afd907b7bd 218 printf("\r\n\r\n======================\r\nStepper test\r\n======================\r\n\r\n");
boro 0:41afd907b7bd 219 // test stepper motor
boro 0:41afd907b7bd 220 enableStepper = 0;
boro 0:41afd907b7bd 221 wait(0.5f);
boro 0:41afd907b7bd 222 printf("stepper enabled\r\n");
boro 0:41afd907b7bd 223 stepperMotor.goesTo(20*2048);
boro 0:41afd907b7bd 224 stepperMotor2.goesTo(20*2048);
boro 0:41afd907b7bd 225 wait(3.0f);
boro 0:41afd907b7bd 226 stepperMotor.stop();
boro 0:41afd907b7bd 227 stepperMotor2.stop();
boro 0:41afd907b7bd 228 stepperMotor.setPositionZero();
boro 0:41afd907b7bd 229 stepperMotor2.setPositionZero();
boro 0:41afd907b7bd 230 enableStepper = 1;
boro 0:41afd907b7bd 231 printf("stepper done\r\n");
boro 0:41afd907b7bd 232 done = true;
boro 0:41afd907b7bd 233 break;
boro 0:41afd907b7bd 234 } else {
boro 0:41afd907b7bd 235 break;
boro 0:41afd907b7bd 236 }
boro 0:41afd907b7bd 237
boro 0:41afd907b7bd 238 case 7:
boro 0:41afd907b7bd 239 // ===================
boro 0:41afd907b7bd 240 // SD
boro 0:41afd907b7bd 241 // ===================
boro 0:41afd907b7bd 242 if(!done){
boro 0:41afd907b7bd 243 printf("\r\n\r\n======================\r\nSD test\r\n======================\r\n\r\n");
boro 0:41afd907b7bd 244
boro 0:41afd907b7bd 245 // crearte folder
boro 0:41afd907b7bd 246 mkdir("/sd/mydir", 0777);
boro 0:41afd907b7bd 247
boro 0:41afd907b7bd 248 // create file
boro 0:41afd907b7bd 249 printf("start write SD\r\n");
boro 0:41afd907b7bd 250 FILE *fp = fopen("/sd/mydir/sdtest.txt", "w");
boro 0:41afd907b7bd 251 if(fp == NULL) {
boro 0:41afd907b7bd 252 error("Could not open file for write\r\n");
boro 0:41afd907b7bd 253 }
boro 0:41afd907b7bd 254 fprintf(fp, "analog measure\r\n");
boro 0:41afd907b7bd 255 fprintf(fp, "==============\r\n");
boro 0:41afd907b7bd 256 fclose(fp);
boro 0:41afd907b7bd 257
boro 0:41afd907b7bd 258 // write 2 times an analog value
boro 0:41afd907b7bd 259 for(int i=0; i<2; i++){
boro 0:41afd907b7bd 260 sdWrite(ain.read());
boro 0:41afd907b7bd 261 wait(2.0f);
boro 0:41afd907b7bd 262 }
boro 0:41afd907b7bd 263 // read from SD card
boro 0:41afd907b7bd 264 printf("Reading from SD card...");
boro 0:41afd907b7bd 265 fp = fopen("/sd/mydir/sdtest.txt", "r");
boro 0:41afd907b7bd 266 if (fp != NULL) {
boro 0:41afd907b7bd 267 char c = fgetc(fp);
boro 0:41afd907b7bd 268 if (c == 'a')
boro 0:41afd907b7bd 269 printf("success!\n");
boro 0:41afd907b7bd 270 else
boro 0:41afd907b7bd 271 printf("incorrect char (%c)!\n", c);
boro 0:41afd907b7bd 272 fclose(fp);
boro 0:41afd907b7bd 273 } else {
boro 0:41afd907b7bd 274 printf("failed!\n");
boro 0:41afd907b7bd 275 }
boro 0:41afd907b7bd 276
boro 0:41afd907b7bd 277 done = true;
boro 0:41afd907b7bd 278 break;
boro 0:41afd907b7bd 279 } else {
boro 0:41afd907b7bd 280 break;
boro 0:41afd907b7bd 281 }
boro 0:41afd907b7bd 282
boro 0:41afd907b7bd 283
boro 0:41afd907b7bd 284 default:
boro 0:41afd907b7bd 285 printf("\r\n\r\n======================\r\nPause\r\n======================\r\n\r\n");
boro 0:41afd907b7bd 286 break;
boro 0:41afd907b7bd 287 }
boro 0:41afd907b7bd 288
boro 0:41afd907b7bd 289
boro 0:41afd907b7bd 290 myled = 1;
boro 0:41afd907b7bd 291
boro 0:41afd907b7bd 292 wait(0.1); // 0.5s
boro 0:41afd907b7bd 293
boro 0:41afd907b7bd 294 myled = 0;
boro 0:41afd907b7bd 295 }
boro 0:41afd907b7bd 296 }