mmotor
/
MarsRover_ExoMars____
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: EPOS2 mbed-rtos mbed
Fork of
MarsRover_ExoMars
by 
mmotor
Diff: main.cpp
- Revision:
- 0:aaf583a75b97
- Child:
- 1:c53aafa72b36
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Thu Jul 28 06:24:33 2016 +0000
@@ -0,0 +1,163 @@
+#include "mbed.h"
+#include "EPOS2.h"
+//#include "RobotControl.h"
+
+//***********************************************************************************************************************************//
+//*** Global_Var *******************************************************************************************************************//
+//***********************************************************************************************************************************//
+
+Serial pc(USBTX, USBRX); // (tx, rx)
+
+
+CAN can(p9, p10); // Can Pin def
+
+
+int velocity = 3000;
+int acceleration = 50000;
+int deceleration = 50000;
+
+int anzahl_zyklen = 1000000;
+int absolvierte_zyklen = 0;
+int ActualPos = 0;
+int DemandPos = 0;
+int ActualCurrent = 0;
+int DemandCurrent = 0;
+int counter=0;
+bool start = true;
+bool counter_s =false;
+
+
+
+void Motor1 (EPOS2 MyEpos){
+ pc.printf("Motor1\n");
+
+
+ MyEpos.SetDigOut(3, 0);
+ wait(2);
+
+
+
+
+ // while(1){
+
+
+ // pc.printf("start\n");
+ MyEpos.SetDigOut(3, 1); //(1=Enable, 0=Disable)
+ MyEpos.MoveAbsolute(500,20,500,deceleration);
+ MyEpos.SetDigOut(3, 0); //(TargetPosition, PosSpeed, acceleration, deceleration)
+ wait(1);
+ // pc.printf("%d\n", MyEpos.GetError());
+
+ while (MyEpos.TargetReached() == 0) {
+ ActualPos = MyEpos.GetActualPos();
+ DemandPos = MyEpos.GetDemandPos();
+ ActualCurrent = MyEpos.GetActualCurrentAveraged();
+ DemandCurrent = MyEpos.GetDemandCurrent();
+ //wait_ms(100);
+ MyEpos.SetDigOut(3, 0);
+ } // pc.printf("%10.d,%10.d,%10.hd,%10.hd\n", ActualPos, DemandPos, ActualCurrent, DemandCurrent);
+
+
+ wait(0.5);
+
+ MyEpos.SetDigOut(3, 1);
+ //wait_ms(100);
+ MyEpos.MoveAbsolute(0,3000,3000,3000);
+ MyEpos.SetDigOut(3, 0);
+ wait(1);
+
+
+ while (MyEpos.TargetReached() == 0) {
+ ActualPos = MyEpos.GetActualPos();
+ DemandPos = MyEpos.GetDemandPos();
+ ActualCurrent = MyEpos.GetActualCurrent();
+ DemandCurrent = MyEpos.GetDemandCurrent();
+ //wait_ms(100);
+ MyEpos.SetDigOut(3, 0);
+ //pc.printf("%10.d,%10.d,%10.hd,%10.hd\n", ActualPos, DemandPos, ActualCurrent, DemandCurrent);
+
+ }
+ //}
+}
+
+
+void Motor2(EPOS2 MyEpos2){
+ pc.printf("Motor2\n");
+
+
+
+ wait_ms(100) ;
+ MyEpos2. MoveVelocity(1000, 80, 1000);
+ wait(0.1);
+
+
+
+}
+int main() ///////// Main
+{
+restart:
+ //************** Initialisierung **************//
+ counter_s=false;
+ pc.baud(9600);
+ pc.printf("Fehlerfall %d\n", counter) ;
+ wait(2); //Wartezeit bis Epos2 70/10 aufgestartet ist
+
+ pc.printf("Initialisation CAN\n");
+
+ can.frequency(1000000); //Define Can baud in bit/s
+
+ CANopen canOpen(&can, 0.001); //Define CanOpen Network(can function, periode of the CANopen driver in sec)
+ canOpen.start(); //Start defined CanOpen Network
+
+ EPOS2 MyEpos(&canOpen, 1);
+ EPOS2 MyEpos2(&canOpen, 2); //Define and connect the EPOS2 to the CanOpen Network(canOpen function, Node ID)
+
+ //RobotControl robotControl(&MyEpos, &MyEpos2);
+ //robotControl.start();
+
+
+ MyEpos.SetPar(10,2660, 1, 3100, 2.76); //Set the motor parameter( Motortype, Current limit, Pole pairs, max velocity, thermal time constant winding)
+ MyEpos.SetHomingPar(23, 10, 10, 0); //Set Homing Parameter (homing mode->35=Actual Position, speed, acceleration, offset)
+ MyEpos.Reset(); //Reset the EPOS2
+ wait(2);
+ MyEpos2.SetPar(10,936, 8, 1000, 29.8); //Set the motor parameter( Motortype, Current limit, Pole pairs, max velocity, thermal time constant winding)
+ MyEpos2.SetHomingPar(23, 10, 10, 0); //Set Homing Parameter (homing mode->35=Actual Position, speed, acceleration, offset)
+ MyEpos2.Reset(); //Reset the EPOS2
+ wait(2);
+ pc.printf("Homing\n");
+
+ MyEpos.Power(1);
+ pc.printf("power\n");
+ MyEpos2.Power(1); //Enable the Epos for Homing
+ pc.printf("Power2\n");
+ wait(2);
+ MyEpos.Homing(); //do Homing and wait untill homing is done // MyEpos.Power(1);
+ wait_ms(100);
+
+
+ pc.printf("Initialisation abgeschlossen\n");
+
+ while(1){
+
+ if (MyEpos.GetError()==true || MyEpos2.GetError()==true && counter<3) {
+ pc.printf("FEHLER!!!\n");
+
+ if (counter_s==false){
+ counter++;
+ counter_s=true;
+ }
+ goto restart;
+
+ }
+
+ if(counter>2){
+ MyEpos.Power(0);
+ MyEpos2.Power(0);
+ }
+
+ Motor2(MyEpos2);
+ Motor1(MyEpos);
+
+ // pc.printf("%d\n", MyEpos.GetError());
+ }//while
+}//main
\ No newline at end of file