Michael Ernst Peter / Mbed OS PM2_Example_ROME2_Robots

Example project

Dependencies:   PM2_Libary Eigen

main.cpp@36:23addefb97af, 2022-05-10 (annotated)

Committer:
pmic
Date:
Tue May 10 10:03:51 2022 +0200
Revision:
36:23addefb97af
Parent:
31:1b2a1bd1bccb
Child:
39:f336caef17d9
Included main from bleik

Who changed what in which revision?

UserRevisionLine numberNew contents of line
pmic 36:23addefb97af 1 /*
pmic 36:23addefb97af 2 * Main.cpp
pmic 36:23addefb97af 3 * Copyright (c) 2022, ZHAW
pmic 36:23addefb97af 4 * All rights reserved.
pmic 36:23addefb97af 5 */
pmic 17:c19b471f05cb 6
pmic 36:23addefb97af 7 #include <stdio.h>
pmic 36:23addefb97af 8 #include <mbed.h>
pmic 36:23addefb97af 9 #include "IRSensor.h"
pmic 36:23addefb97af 10 #include "EncoderCounterROME2.h"
pmic 36:23addefb97af 11 #include "Controller.h"
pmic 36:23addefb97af 12 #include "StateMachine.h"
pmic 17:c19b471f05cb 13
pmic 36:23addefb97af 14 int main() {
pmic 36:23addefb97af 15
pmic 36:23addefb97af 16 // create miscellaneous periphery objects
pmic 36:23addefb97af 17
pmic 36:23addefb97af 18 DigitalIn button(BUTTON1);
pmic 36:23addefb97af 19 DigitalOut led(LED1);
pmic 36:23addefb97af 20
pmic 36:23addefb97af 21 DigitalOut led0(PD_4);
pmic 36:23addefb97af 22 DigitalOut led1(PD_3);
pmic 36:23addefb97af 23 DigitalOut led2(PD_6);
pmic 36:23addefb97af 24 DigitalOut led3(PD_2);
pmic 36:23addefb97af 25 DigitalOut led4(PD_7);
pmic 36:23addefb97af 26 DigitalOut led5(PD_5);
pmic 36:23addefb97af 27
pmic 36:23addefb97af 28 // create IR sensor objects
pmic 36:23addefb97af 29
pmic 36:23addefb97af 30 AnalogIn distance(PA_0);
pmic 36:23addefb97af 31 DigitalOut enable(PG_1);
pmic 36:23addefb97af 32 DigitalOut bit0(PF_0);
pmic 36:23addefb97af 33 DigitalOut bit1(PF_1);
pmic 36:23addefb97af 34 DigitalOut bit2(PF_2);
pmic 36:23addefb97af 35
pmic 36:23addefb97af 36 IRSensor irSensor0(distance, bit0, bit1, bit2, 0);
pmic 36:23addefb97af 37 IRSensor irSensor1(distance, bit0, bit1, bit2, 1);
pmic 36:23addefb97af 38 IRSensor irSensor2(distance, bit0, bit1, bit2, 2);
pmic 36:23addefb97af 39 IRSensor irSensor3(distance, bit0, bit1, bit2, 3);
pmic 36:23addefb97af 40 IRSensor irSensor4(distance, bit0, bit1, bit2, 4);
pmic 36:23addefb97af 41 IRSensor irSensor5(distance, bit0, bit1, bit2, 5);
pmic 36:23addefb97af 42
pmic 36:23addefb97af 43 enable = 1;
pmic 36:23addefb97af 44
pmic 36:23addefb97af 45 // create motor control objects
pmic 36:23addefb97af 46
pmic 36:23addefb97af 47 DigitalOut enableMotorDriver(PG_0);
pmic 36:23addefb97af 48 DigitalIn motorDriverFault(PD_1);
pmic 36:23addefb97af 49 DigitalIn motorDriverWarning(PD_0);
pmic 36:23addefb97af 50
pmic 36:23addefb97af 51 PwmOut pwmLeft(PF_9);
pmic 36:23addefb97af 52 PwmOut pwmRight(PF_8);
pmic 36:23addefb97af 53
pmic 36:23addefb97af 54 EncoderCounterROME2 counterLeft(PD_12, PD_13);
pmic 36:23addefb97af 55 EncoderCounterROME2 counterRight(PB_4, PC_7);
pmic 36:23addefb97af 56
pmic 36:23addefb97af 57 // create robot controller objects
pmic 36:23addefb97af 58
pmic 36:23addefb97af 59 Controller controller(pwmLeft, pwmRight, counterLeft, counterRight);
pmic 36:23addefb97af 60 StateMachine stateMachine(controller, enableMotorDriver, led0, led1, led2, led3, led4, led5, button, irSensor0, irSensor1, irSensor2, irSensor3, irSensor4, irSensor5);
pmic 36:23addefb97af 61
pmic 36:23addefb97af 62 while (true) {
pmic 36:23addefb97af 63
pmic 36:23addefb97af 64 led = !led;
pmic 36:23addefb97af 65
pmic 36:23addefb97af 66 printf("actual velocity: %.3f [m/s] / %.3f [rad/s]\r\n", controller.getActualTranslationalVelocity(), controller.getActualRotationalVelocity());
pmic 36:23addefb97af 67
pmic 36:23addefb97af 68 ThisThread::sleep_for(100ms);
pmic 1:93d997d6b232 69 }
pmic 1:93d997d6b232 70 }