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?
User | Revision | Line number | New 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 | } |