Solar Boat Twente / Mbed 2 deprecated BIEM_Test_Wings

Programm for checking if the wings are properly calibrated

Dependencies:   Epos_control mbed-rtos mbed

main.cpp@4:6e11f0057362, 2017-08-23 (annotated)

Committer:
BramS23
Date:
Wed Aug 23 10:24:55 2017 +0000
Revision:
4:6e11f0057362
Parent:
3:d404323036d5 
ya know

Who changed what in which revision?

UserRevisionLine numberNew contents of line
BramS23 0:36ef9331276c 1 #include "mbed.h"
BramS23 0:36ef9331276c 2 #include "rtos.h"
BramS23 0:36ef9331276c 3 #include "EPOS.h"
BramS23 0:36ef9331276c 4
BramS23 0:36ef9331276c 5
BramS23 0:36ef9331276c 6 RawSerial pc(SERIAL_TX,SERIAL_RX);
BramS23 0:36ef9331276c 7 CAN can(PB_5,PB_6);
BramS23 0:36ef9331276c 8 EPOS epos1;
BramS23 0:36ef9331276c 9 EPOS epos2;
BramS23 0:36ef9331276c 10 EPOS epos3;
BramS23 0:36ef9331276c 11 EPOS epos4;
BramS23 0:36ef9331276c 12 EPOS epos5;
BramS23 0:36ef9331276c 13 Thread thread1;
BramS23 0:36ef9331276c 14 Thread thread2;
BramS23 0:36ef9331276c 15 Thread thread3;
BramS23 0:36ef9331276c 16 Thread thread4;
BramS23 0:36ef9331276c 17 Thread thread5;
BramS23 0:36ef9331276c 18 float quadc1;
BramS23 0:36ef9331276c 19 float quadc2;
BramS23 0:36ef9331276c 20 float quadc3;
BramS23 0:36ef9331276c 21 float quadc4;
BramS23 0:36ef9331276c 22
BramS23 0:36ef9331276c 23
BramS23 0:36ef9331276c 24 int main() {
BramS23 2:80d80eadf319 25 pc.baud(921600);
BramS23 2:80d80eadf319 26 can.frequency(250000);
BramS23 0:36ef9331276c 27 pc.printf("startup: \r\n");
BramS23 3:d404323036d5 28
BramS23 4:6e11f0057362 29
BramS23 3:d404323036d5 30
BramS23 0:36ef9331276c 31 epos1.Setup_EPOS(1);
BramS23 0:36ef9331276c 32 epos2.Setup_EPOS(2);
BramS23 0:36ef9331276c 33 epos3.Setup_EPOS(3);
BramS23 0:36ef9331276c 34 epos4.Setup_EPOS(4);
BramS23 0:36ef9331276c 35 epos5.Setup_EPOS(5);
BramS23 0:36ef9331276c 36
BramS23 4:6e11f0057362 37 epos1.Start_pos_mode();Thread::wait(10);
BramS23 4:6e11f0057362 38 epos2.Start_pos_mode();Thread::wait(10);
BramS23 4:6e11f0057362 39 epos3.Start_pos_mode();Thread::wait(10);
BramS23 4:6e11f0057362 40 epos4.Start_pos_mode();Thread::wait(10);
BramS23 4:6e11f0057362 41 epos5.Start_pos_mode();Thread::wait(10);
BramS23 4:6e11f0057362 42
BramS23 0:36ef9331276c 43 thread1.start(&epos1,&EPOS::Homing);
BramS23 0:36ef9331276c 44 Thread::wait(5);
BramS23 0:36ef9331276c 45 thread2.start(&epos2,&EPOS::Homing);
BramS23 0:36ef9331276c 46 Thread::wait(5);
BramS23 0:36ef9331276c 47 thread3.start(&epos3,&EPOS::Homing);
BramS23 0:36ef9331276c 48 Thread::wait(5);
BramS23 0:36ef9331276c 49 thread4.start(&epos4,&EPOS::Homing);
BramS23 0:36ef9331276c 50 Thread::wait(5);
BramS23 0:36ef9331276c 51 thread5.start(&epos5,&EPOS::Homing);
BramS23 0:36ef9331276c 52
BramS23 0:36ef9331276c 53 Thread::wait(11000);
BramS23 0:36ef9331276c 54
BramS23 2:80d80eadf319 55 epos1.Start_pos_mode();Thread::wait(10);
BramS23 2:80d80eadf319 56 epos2.Start_pos_mode();Thread::wait(10);
BramS23 2:80d80eadf319 57 epos3.Start_pos_mode();Thread::wait(10);
BramS23 2:80d80eadf319 58 epos4.Start_pos_mode();Thread::wait(10);
BramS23 2:80d80eadf319 59 epos5.Start_pos_mode();Thread::wait(10);
BramS23 2:80d80eadf319 60
BramS23 0:36ef9331276c 61 pc.printf("epos probably homed \r\n");
BramS23 0:36ef9331276c 62
BramS23 0:36ef9331276c 63 // epos1 vleugel BB
BramS23 0:36ef9331276c 64 // epos2en3 vleugel midden
BramS23 0:36ef9331276c 65 // epos4 vleugel SB
BramS23 0:36ef9331276c 66
BramS23 1:4cc044084acf 67 float AoA1_offset = 0.0f;
BramS23 1:4cc044084acf 68 float AoA2_offset = 0.0f;
BramS23 1:4cc044084acf 69 float AoA3_offset = 0.0f;
BramS23 1:4cc044084acf 70 float AoA4_offset = 0.0f;
BramS23 1:4cc044084acf 71
BramS23 0:36ef9331276c 72 //-------------------------------------------------------------------------
BramS23 0:36ef9331276c 73 pc.printf("going to first position \r\n");
BramS23 0:36ef9331276c 74
BramS23 0:36ef9331276c 75
BramS23 1:4cc044084acf 76 float AoA1 = -3.0f; // AoA BB (deg)
BramS23 1:4cc044084acf 77 float AoA2 = 0.0f; // AoA mid (deg)
BramS23 1:4cc044084acf 78 float AoA3 = -3.0f; // AoA SB
BramS23 1:4cc044084acf 79 float AoA4 = 11.0f; // AoA rear
BramS23 1:4cc044084acf 80
BramS23 1:4cc044084acf 81 quadc1 = (int)(13274*(AoA1 +AoA1_offset)); // Determine the quadcounts for EPOS
BramS23 1:4cc044084acf 82 quadc2 = (int)(-56757*(AoA2 +AoA2_offset)); // 1 degree of AoA approximates
BramS23 1:4cc044084acf 83 quadc3 = (int)(13274*(AoA3 +AoA3_offset)); // 52113 quadcounts
BramS23 1:4cc044084acf 84 quadc4 = (int)(70796*(AoA4 +AoA4_offset)); // 52113 quadcounts
BramS23 0:36ef9331276c 85
BramS23 0:36ef9331276c 86 can.write(epos1.Goto_Pos(quadc1)); // send new position to the controllers
BramS23 0:36ef9331276c 87 Thread::wait(1);
BramS23 0:36ef9331276c 88 can.write(epos2.Goto_Pos(quadc2));
BramS23 0:36ef9331276c 89 Thread::wait(1);
BramS23 0:36ef9331276c 90 can.write(epos3.Goto_Pos(quadc2));
BramS23 0:36ef9331276c 91 Thread::wait(1);
BramS23 0:36ef9331276c 92 can.write(epos4.Goto_Pos(quadc3));
BramS23 0:36ef9331276c 93 Thread::wait(1);
BramS23 0:36ef9331276c 94 can.write(epos5.Goto_Pos(quadc4));
BramS23 0:36ef9331276c 95 Thread::wait(1);
BramS23 0:36ef9331276c 96
BramS23 0:36ef9331276c 97 Thread::wait(10000);
BramS23 0:36ef9331276c 98
BramS23 0:36ef9331276c 99 //-------------------------------------------------------------------------
BramS23 0:36ef9331276c 100 pc.printf("going to second position \r\n");
BramS23 0:36ef9331276c 101 AoA1 = 0.0f; // AoA BB (deg)
BramS23 1:4cc044084acf 102 AoA2 = 2.5f; // AoA mid (deg)
BramS23 0:36ef9331276c 103 AoA3 = 0.0f; // AoA SB
BramS23 1:4cc044084acf 104 AoA4 = 11.0f; // AoA rear
BramS23 0:36ef9331276c 105
BramS23 1:4cc044084acf 106 quadc1 = (int)(13274*(AoA1 +AoA1_offset)); // Determine the quadcounts for EPOS
BramS23 1:4cc044084acf 107 quadc2 = (int)(-56757*(AoA2 +AoA2_offset)); // 1 degree of AoA approximates
BramS23 1:4cc044084acf 108 quadc3 = (int)(13274*(AoA3 +AoA3_offset)); // 52113 quadcounts
BramS23 1:4cc044084acf 109 quadc4 = (int)(70796*(AoA4 +AoA4_offset)); // 52113 quadcounts
BramS23 0:36ef9331276c 110
BramS23 0:36ef9331276c 111 can.write(epos1.Goto_Pos(quadc1)); // send new position to the controllers
BramS23 0:36ef9331276c 112 Thread::wait(1);
BramS23 0:36ef9331276c 113 can.write(epos2.Goto_Pos(quadc2));
BramS23 0:36ef9331276c 114 Thread::wait(1);
BramS23 0:36ef9331276c 115 can.write(epos3.Goto_Pos(quadc2));
BramS23 0:36ef9331276c 116 Thread::wait(1);
BramS23 0:36ef9331276c 117 can.write(epos4.Goto_Pos(quadc3));
BramS23 0:36ef9331276c 118 Thread::wait(1);
BramS23 0:36ef9331276c 119 can.write(epos5.Goto_Pos(quadc4));
BramS23 0:36ef9331276c 120 Thread::wait(1);
BramS23 0:36ef9331276c 121
BramS23 0:36ef9331276c 122 //-------------------------------------------------------------------------
BramS23 0:36ef9331276c 123 pc.printf("going to thrid position \r\n");
BramS23 1:4cc044084acf 124 AoA1 = 2.5f; // AoA BB (deg)
BramS23 1:4cc044084acf 125 AoA2 = 5.0f; // AoA mid (deg)
BramS23 1:4cc044084acf 126 AoA3 = 2.5f; // AoA SB
BramS23 1:4cc044084acf 127 AoA4 = 11.0f; // AoA rear
BramS23 0:36ef9331276c 128
BramS23 1:4cc044084acf 129 quadc1 = (int)(13274*(AoA1 +AoA1_offset)); // Determine the quadcounts for EPOS
BramS23 1:4cc044084acf 130 quadc2 = (int)(-56757*(AoA2 +AoA2_offset)); // 1 degree of AoA approximates
BramS23 1:4cc044084acf 131 quadc3 = (int)(13274*(AoA3 +AoA3_offset)); // 52113 quadcounts
BramS23 1:4cc044084acf 132 quadc4 = (int)(70796*(AoA4 +AoA4_offset)); // 52113 quadcounts
BramS23 0:36ef9331276c 133
BramS23 0:36ef9331276c 134 can.write(epos1.Goto_Pos(quadc1)); // send new position to the controllers
BramS23 0:36ef9331276c 135 Thread::wait(1);
BramS23 0:36ef9331276c 136 can.write(epos2.Goto_Pos(quadc2));
BramS23 0:36ef9331276c 137 Thread::wait(1);
BramS23 0:36ef9331276c 138 can.write(epos3.Goto_Pos(quadc2));
BramS23 0:36ef9331276c 139 Thread::wait(1);
BramS23 0:36ef9331276c 140 can.write(epos4.Goto_Pos(quadc3));
BramS23 0:36ef9331276c 141 Thread::wait(1);
BramS23 0:36ef9331276c 142 can.write(epos5.Goto_Pos(quadc4));
BramS23 0:36ef9331276c 143 Thread::wait(1);
BramS23 0:36ef9331276c 144
BramS23 0:36ef9331276c 145 //-------------------------------------------------------------------------
BramS23 0:36ef9331276c 146 pc.printf("going to fourth position \r\n");
BramS23 1:4cc044084acf 147 AoA1 = 5.0f; // AoA BB (deg)
BramS23 1:4cc044084acf 148 AoA2 = 10.0f; // AoA mid (deg)
BramS23 1:4cc044084acf 149 AoA3 = 5.0f; // AoA SB
BramS23 1:4cc044084acf 150 AoA4 = 11.0f; // AoA rear
BramS23 0:36ef9331276c 151
BramS23 1:4cc044084acf 152 quadc1 = (int)(13274*(AoA1 +AoA1_offset)); // Determine the quadcounts for EPOS
BramS23 1:4cc044084acf 153 quadc2 = (int)(-56757*(AoA2 +AoA2_offset)); // 1 degree of AoA approximates
BramS23 1:4cc044084acf 154 quadc3 = (int)(13274*(AoA3 +AoA3_offset)); // 52113 quadcounts
BramS23 1:4cc044084acf 155 quadc4 = (int)(70796*(AoA4 +AoA4_offset)); // 52113 quadcounts
BramS23 0:36ef9331276c 156
BramS23 0:36ef9331276c 157 can.write(epos1.Goto_Pos(quadc1)); // send new position to the controllers
BramS23 0:36ef9331276c 158 Thread::wait(1);
BramS23 0:36ef9331276c 159 can.write(epos2.Goto_Pos(quadc2));
BramS23 0:36ef9331276c 160 Thread::wait(1);
BramS23 0:36ef9331276c 161 can.write(epos3.Goto_Pos(quadc2));
BramS23 0:36ef9331276c 162 Thread::wait(1);
BramS23 0:36ef9331276c 163 can.write(epos4.Goto_Pos(quadc3));
BramS23 0:36ef9331276c 164 Thread::wait(1);
BramS23 0:36ef9331276c 165 can.write(epos5.Goto_Pos(quadc4));
BramS23 0:36ef9331276c 166 Thread::wait(1);
BramS23 0:36ef9331276c 167 }