PRISMA Lab
Clone per testare Mbed studio
Dependencies: PwmIn IONMcMotor MPU6050 Eigen ROVER
Fork of
Hyfliers_Completo_testato
by 
Marco De Silva
Diff: main.cpp
- Revision:
- 3:fc26045926d9
- Parent:
- 1:ec61ea9f67de
- Child:
- 4:3f22193053d0
--- a/main.cpp Mon Jun 26 09:28:54 2017 +0000
+++ b/main.cpp Wed Nov 06 10:57:51 2019 +0000
@@ -1,54 +1,159 @@
-#if !FEATURE_LWIP
- #error [NOT_SUPPORTED] LWIP not supported for this target
-#endif
+#include "mbed.h"
+#include "Servo.h"
+#include "Rover.h"
+#include "math.h"
+#include "TOFs.h"
+//#include "MX.h"
+//#include "UARTSerial_half.h"
-#include "mbed.h"
-#include "EthernetInterface.h"
-#include "TCPServer.h"
-#include "TCPSocket.h"
+#define DEBUG false
+#define READ true
+
+
+DigitalOut myled1(LED1);
+DigitalOut myled2(LED2);
+
+//UARTSerial_half dxl_port(PB_9,PB_8,PF_13, 1000000);
+
+//int ID[5] = {1,2,3,4,5};
+//MX mx_MotorChain(ID, sizeof(ID)/sizeof(int), 1000000, &dxl_port);
-#define HTTP_STATUS_LINE "HTTP/1.0 200 OK"
-#define HTTP_HEADER_FIELDS "Content-Type: text/html; charset=utf-8"
-#define HTTP_MESSAGE_BODY "" \
-"<html>" "\r\n" \
-" <body style=\"display:flex;text-align:center\">" "\r\n" \
-" <div style=\"margin:auto\">" "\r\n" \
-" <h1>Hello World</h1>" "\r\n" \
-" <p>It works !</p>" "\r\n" \
-" </div>" "\r\n" \
-" </body>" "\r\n" \
-"</html>"
+int main() {
+
+
+ /* bool enableVal[5] = {1,1,1,1,1};
+ mx_MotorChain.SyncTorqueEnable(enableVal);
+
+ float Goal_position[5] = {100,20,150,60,75};
+ mx_MotorChain.SyncSetGoal(Goal_position); */
+
+ float velf_a;
+ float vels_a;
+ float vela_a;
+ float velf_m = 0.0;
+ float vels_m = 0.0;
+ float vela_m = 0.0;
+
+ float vel_dx = 0.0;
+ float vel_sx = 0.0;
+ float vel_r = 0.0;
+ float forward_vel = 0.0;
+ float pitch;
+
+ float jointFront_d = 0.0;
+ float jointRetro_d = 0.0;
+ float time = 0.0;
+
+
+ float roll;
+ printf("Creo il rover \r\n");
+ Rover rover;
+
+
+ rover.initializeImu();
+ rover.initializeTofs();
-#define HTTP_RESPONSE HTTP_STATUS_LINE "\r\n" \
- HTTP_HEADER_FIELDS "\r\n" \
- "\r\n" \
- HTTP_MESSAGE_BODY "\r\n"
+
+ Timer schedulerTimer;
+
+ schedulerTimer.start();
+
+ rover.setCentralJointsAngle(0.0,0.0); //in radiants (front, retro)
+ rover.setWheelsVelocity(0.0,0.0 , 0, 0.125, 10000); //velForward, velStabilization, velAsset, pipeRadious, acceleration
+
+ rover.startEthComunication();
+
+ wait(1);
+
+ double timeStabPrec = 0.0;
+ double timeReadPrec = 0.0;
+ double timePrintPrec = 0.0;
+
+ timeStabPrec = schedulerTimer.read();
+ timePrintPrec = schedulerTimer.read();
+ timeReadPrec = schedulerTimer.read();
-int main()
-{
- printf("Basic HTTP server example\n");
+ float dtStab = 5.0/1000;
+ float dtRead = 20.0/1000;
+ float dtPrint = 500.0/1000;
+ float dtPassedStab = 0.0;
+ float Kp=1.2; //0.6
- EthernetInterface eth;
- eth.connect();
-
- printf("The target IP address is '%s'\n", eth.get_ip_address());
-
- TCPServer srv;
- TCPSocket clt_sock;
- SocketAddress clt_addr;
+ float pitch_d = 0.0;
- /* Open the server on ethernet stack */
- srv.open(ð);
-
- /* Bind the HTTP port (TCP 80) to the server */
- srv.bind(eth.get_ip_address(), 80);
-
- /* Can handle 5 simultaneous connections */
- srv.listen(5);
-
- while (true) {
- srv.accept(&clt_sock, &clt_addr);
- printf("accept %s:%d\n", clt_addr.get_ip_address(), clt_addr.get_port());
- clt_sock.send(HTTP_RESPONSE, strlen(HTTP_RESPONSE));
- }
+ float frontDistance;
+ float retroDistance;
+
+ while (1){
+
+ time = schedulerTimer.read();
+
+
+ if(time - timeStabPrec > dtStab){
+
+ //rover.acquireTofs(frontDistance, retroDistance);
+
+ rover.computeCentralJointsFromTofs();
+ dtPassedStab = time - timeStabPrec;
+ timeStabPrec = time;
+
+ rover.calcImuAngles(pitch, roll, dtStab);
+
+ velf_a = forward_vel;
+ vels_a = -Kp*M_PI*(pitch_d-pitch)/180;
+ vela_a = 0.0;
+
+ rover.setWheelsVelocity(velf_a, vels_a , vela_a, 0.125, 10000); //velForward, velStabilization, velAsset, pipeRadious, acceleration
+ //rover.getRoverVelocity(velf_m, vels_m, vela_m, 0.125);
+
+ forward_vel = (float)rover.get_forward_vel()/1000;
+ pitch_d = (float)rover.get_pitch()/1000;
+
+ jointFront_d = (float)rover.get_jointFront()/1000;
+ jointRetro_d = (float)rover.get_jointRetro()/1000;
+
+ //rover.setCentralJointsAngle(jointFront_d,jointRetro_d); //in radiants (front, retro)
+
+
+ }
+
+ if(READ && time - timeReadPrec > dtRead){
+
+ timeReadPrec = time;
+ //rover.getRoverVelocity(velf_m, vels_m, vela_m, 0.125);
+ rover.getRoverWheelsVelocity(vel_dx, vel_sx, vel_r);
+
+
+ }
+
+
+ if(DEBUG && time - timePrintPrec > dtPrint){
+ timePrintPrec = time;
+
+ //printf("Ts: %4.2f \t M2-M1 : %2.1f \r\n",dtPassedStab*1000,frontDistance);
+
+ //printf(" Ts: %4.2f \t Vfa: %4.4f \t Vfm %4.4f \t Vsa: %4.4f \t Vsm: %4.4f \t A: %4.4f \r\n",dtPassedStab*1000, velf_a, velf_m, vels_a, vels_m, amplitude);
+
+ if(rover.getEthState() == 1){
+ myled1 = true;
+ myled2 = false;
+ }else if(rover.getEthState() == 2){
+ myled1 = true;
+ myled2 = true;
+ }else{
+ myled1 = false;
+ myled2 = false;
+ }
+
+
+
+ }
+ rover.ethComunicationUpdate(schedulerTimer.read(), pitch, frontDistance, retroDistance, dtPassedStab*1000, vel_r);
+ //rover.ethComunicationUpdate(schedulerTimer.read(), pitch, velf_m, vels_m, dtPassedStab*1000, vel_r);
+
+
+
+ }
+
}
+