aigamozu / AigamozuControlPackets_2016

2016_05_19ver Auto mode 10sec forward, 2sec stop, 2sec right turn Please change test_mode's right turn ppm

Dependencies:   VNH5019 AigamozuControlPackets_2016

Dependents:   Aigamozu_Robot_2016_ver1 GPSLOG_program AigamozuControlPackets_2016

Fork of AigamozuControlPackets by aigamozu

AigamozuControlPackets.cpp@50:3511be172d81, 2017-10-29 (annotated)

Committer:
s1210160
Date:
Sun Oct 29 00:56:51 2017 +0000
Revision:
50:3511be172d81
Parent:
48:ee5a6906273e 
commit;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
m5171135 0:2b8b56ac7a82 1 #include "AigamozuControlPackets.h"
m5171135 0:2b8b56ac7a82 2
s1210160 50:3511be172d81 3 /////////////////////////////////////////
s1210160 50:3511be172d81 4 //
s1210160 50:3511be172d81 5 // Constructor
s1210160 50:3511be172d81 6 //
s1210160 50:3511be172d81 7 /////////////////////////////////////////
s1210160 50:3511be172d81 8 AigamozuControlPackets::AigamozuControlPackets(VNH5019 agzSheild):_agzSheild(agzSheild)
s1210160 50:3511be172d81 9 {
m5171135 2:3f2d4f53ceed 10 packetData = new uint8_t[50];
m5171135 2:3f2d4f53ceed 11 packetLength = 0;
s1210160 50:3511be172d81 12
s1210160 50:3511be172d81 13 agzPoint.x = 0.0;
s1210160 50:3511be172d81 14 agzPoint.y = 0.0;
s1210160 50:3511be172d81 15
s1210160 50:3511be172d81 16 for(int i=0; i<BASENUMBER; i++) {
s1210160 50:3511be172d81 17 basePoint[i].x = 0.0;
s1210160 50:3511be172d81 18 basePoint[i].y = 0.0;
s1210160 50:3511be172d81 19 }
s1210160 50:3511be172d81 20 }
s1210160 50:3511be172d81 21
s1210160 50:3511be172d81 22 AigamozuControlPackets::vector2D AigamozuControlPackets::sub_vector(vector2D A, vector2D B)
s1210160 50:3511be172d81 23 {
s1210160 50:3511be172d81 24 vector2D ret;
s1210160 50:3511be172d81 25 ret.x = B.x - A.x;
s1210160 50:3511be172d81 26 ret.y = B.y - A.y;
s1210160 50:3511be172d81 27 return ret;
s1210160 50:3511be172d81 28 }
s1210160 50:3511be172d81 29
s1210160 50:3511be172d81 30 bool AigamozuControlPackets::checkInOut(vector2D A, vector2D B, vector2D C, vector2D P)
s1210160 50:3511be172d81 31 {
s1210160 50:3511be172d81 32 vector2D BA = sub_vector(B, A);
s1210160 50:3511be172d81 33 vector2D BP = sub_vector(B, P);
s1210160 50:3511be172d81 34 vector2D BC = sub_vector(B, C);
s1210160 50:3511be172d81 35
s1210160 50:3511be172d81 36 double c1 = BA.x * BP.y - BA.y * BP.x;
s1210160 50:3511be172d81 37 double c2 = BP.x * BC.y - BP.y * BC.x;
s1210160 50:3511be172d81 38
s1210160 50:3511be172d81 39 if((c1>0 && c2>0) || (c1<0 && c2<0)) return true;
s1210160 50:3511be172d81 40 else return false;
s1210160 50:3511be172d81 41 }
s1210160 50:3511be172d81 42
s1210160 50:3511be172d81 43 void AigamozuControlPackets::autoMoving(void)
s1210160 50:3511be172d81 44 {
s1210160 50:3511be172d81 45
s1210160 50:3511be172d81 46 if(checkInOut(basePoint[0], basePoint[1], basePoint[2], agzPoint)
s1210160 50:3511be172d81 47 && checkInOut(basePoint[2], basePoint[3], basePoint[0], agzPoint)) {
s1210160 50:3511be172d81 48 out_flag = false;
s1210160 50:3511be172d81 49 } else {
s1210160 50:3511be172d81 50 out_flag = true;
m5171135 2:3f2d4f53ceed 51 }
m5171135 2:3f2d4f53ceed 52
kityann 17:a6fa8cc96d94 53 }
s1210160 50:3511be172d81 54
s1210160 50:3511be172d81 55 /////////////////////////////////////////
s1210160 50:3511be172d81 56 //
s1210160 50:3511be172d81 57 // Create Packet: Robot -> Base,
s1210160 50:3511be172d81 58 // Manager -> Robot/Base
s1210160 50:3511be172d81 59 //
s1210160 50:3511be172d81 60 /////////////////////////////////////////
s1210160 50:3511be172d81 61 void AigamozuControlPackets::createRequestCommand(uint8_t fromID, uint8_t toID)
s1210160 50:3511be172d81 62 {
s1210160 50:3511be172d81 63 uint8_t tmp[] = {'A','G','S','S','F',fromID,'T',toID,'A','G','E'};
s1210160 50:3511be172d81 64 for(int i = 0; i < REQUEST_COMMNAD_LENGTH; ++i) packetData[i] = tmp[i];
s1210160 50:3511be172d81 65 packetLength = REQUEST_COMMNAD_LENGTH;
kityann 17:a6fa8cc96d94 66 }
s1200058 20:fec2d6dec897 67
s1210160 50:3511be172d81 68 /////////////////////////////////////////
s1210160 50:3511be172d81 69 //
s1210160 50:3511be172d81 70 // Create Packet: Base -> Robot/Manager,
s1210160 50:3511be172d81 71 // Robot -> Manager
s1210160 50:3511be172d81 72 //
s1210160 50:3511be172d81 73 /////////////////////////////////////////
s1210160 50:3511be172d81 74 void AigamozuControlPackets::createReceiveStatusCommand(uint8_t fromID, uint8_t toID, int state)
m5171135 0:2b8b56ac7a82 75 {
s1210160 50:3511be172d81 76 UNION_double_char latitude_data,longitude_data;
m5171135 2:3f2d4f53ceed 77
s1210160 50:3511be172d81 78 latitude_data.double_value=agzPoint.y;
s1210160 50:3511be172d81 79 longitude_data.double_value=agzPoint.x;
s1210160 50:3511be172d81 80
m5171135 2:3f2d4f53ceed 81
s1210160 50:3511be172d81 82 uint8_t tmp[] = {'A', 'G', 'S', 'R', 'F', fromID, 'T', toID, 'S', state,'G', 'P', 'S',
s1210160 50:3511be172d81 83 latitude_data.char_value[0], latitude_data.char_value[1], latitude_data.char_value[2], latitude_data.char_value[3],
s1210160 50:3511be172d81 84 latitude_data.char_value[4], latitude_data.char_value[5], latitude_data.char_value[6], latitude_data.char_value[7],
s1210160 50:3511be172d81 85 longitude_data.char_value[0], longitude_data.char_value[1], longitude_data.char_value[2], longitude_data.char_value[3],
s1210160 50:3511be172d81 86 longitude_data.char_value[4], longitude_data.char_value[5], longitude_data.char_value[6], longitude_data.char_value[7],
s1210160 50:3511be172d81 87 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
s1210160 50:3511be172d81 88 'A', 'G', 'E'
s1210160 50:3511be172d81 89 };
m5171135 2:3f2d4f53ceed 90 for(int i = 0; i < RECEIVE_STATUS_COMMNAD_LENGTH; ++i) packetData[i] = tmp[i];
m5171135 2:3f2d4f53ceed 91 packetLength = RECEIVE_STATUS_COMMNAD_LENGTH;
m5171135 0:2b8b56ac7a82 92 }
m5171135 0:2b8b56ac7a82 93
s1210160 50:3511be172d81 94 /////////////////////////////////////////
s1210160 50:3511be172d81 95 //
s1210160 50:3511be172d81 96 // Update data
s1210160 50:3511be172d81 97 //
s1210160 50:3511be172d81 98 /////////////////////////////////////////
s1210160 50:3511be172d81 99 void AigamozuControlPackets::reNewRobotPoint(long latitudeH, long latitudeL, long longitudeH, long longitudeL)
s1210160 50:3511be172d81 100 {
s1210160 50:3511be172d81 101 agzPoint.x = (double)longitudeH + (double)(longitudeL / 10000.0/60.0);
s1210160 50:3511be172d81 102 agzPoint.y = (double)latitudeH +(double)(latitudeL / 10000.0/60.0);
s1200058 37:5654e31ad452 103 }
m5171135 7:200ce5c1f486 104
s1210160 50:3511be172d81 105 void AigamozuControlPackets::reNewBasePoint(int id, uint8_t *latitude, uint8_t *longitude)
m5171135 7:200ce5c1f486 106 {
s1210160 50:3511be172d81 107 UNION_double_char latitude_data,longitude_data;
s1210160 50:3511be172d81 108 for(int i = 0; i < 8; i++) {
s1210160 50:3511be172d81 109 latitude_data.char_value[i]=latitude[i];
s1210160 50:3511be172d81 110 }
s1210160 50:3511be172d81 111 for(int i = 0; i < 8; i++) {
s1210160 50:3511be172d81 112 longitude_data.char_value[i]=longitude[i];
s1210160 50:3511be172d81 113 }
s1210160 50:3511be172d81 114 basePoint[id].x = longitude_data.double_value;
s1210160 50:3511be172d81 115 basePoint[id].y = latitude_data.double_value;
s1210160 50:3511be172d81 116 }
m5171135 7:200ce5c1f486 117
s1210160 50:3511be172d81 118 /////////////////////////////////////////
s1210160 50:3511be172d81 119 //
s1210160 50:3511be172d81 120 // Manual Mode
s1210160 50:3511be172d81 121 //
s1210160 50:3511be172d81 122 /////////////////////////////////////////
s1210160 50:3511be172d81 123 void AigamozuControlPackets::manualMode() {}
m5171135 5:3f51eeb5aedc 124
s1210160 50:3511be172d81 125 /////////////////////////////////////////
s1210160 50:3511be172d81 126 //
s1210160 50:3511be172d81 127 // Change Mode
s1210160 50:3511be172d81 128 //
s1210160 50:3511be172d81 129 /////////////////////////////////////////
s1210160 50:3511be172d81 130 void AigamozuControlPackets::changeMode(uint8_t *buf)
s1210160 50:3511be172d81 131 {
s1210160 50:3511be172d81 132
m5171135 6:f164a716be46 133 //reset
m5171135 5:3f51eeb5aedc 134 _agzSheild.changeSpeed(0,0,0,0);
m5171135 6:f164a716be46 135 eachModeInt.detach();
m5171135 5:3f51eeb5aedc 136
m5171135 6:f164a716be46 137 //Select Mode
s1210160 50:3511be172d81 138 switch(buf[8]) {
m5171135 2:3f2d4f53ceed 139 case 0:
m5171135 2:3f2d4f53ceed 140 nowMode = STANDBY_MODE;
s1210160 50:3511be172d81 141 break;
s1210160 50:3511be172d81 142
m5171135 2:3f2d4f53ceed 143 case 1:
m5171135 6:f164a716be46 144 eachModeInt.attach(this,&AigamozuControlPackets::manualMode,1.0);
m5171135 2:3f2d4f53ceed 145 nowMode = MANUAL_MODE;
s1210160 50:3511be172d81 146 break;
s1210160 50:3511be172d81 147
m5171135 2:3f2d4f53ceed 148 case 2:
s1210160 50:3511be172d81 149 nowMode = AUTO_MODE;
s1210160 50:3511be172d81 150 //Move_Timer.reset();
s1210160 50:3511be172d81 151 break;
s1210160 50:3511be172d81 152
m5171135 6:f164a716be46 153 default:
m5171135 6:f164a716be46 154 nowMode = STANDBY_MODE;
s1210160 50:3511be172d81 155 break;
s1210160 50:3511be172d81 156
s1210160 50:3511be172d81 157 }
s1210160 50:3511be172d81 158 }
s1210160 50:3511be172d81 159 /////////////////////////////////////////
s1210160 50:3511be172d81 160 //
s1210160 50:3511be172d81 161 // Set
s1210160 50:3511be172d81 162 //
s1210160 50:3511be172d81 163 /////////////////////////////////////////
s1210160 50:3511be172d81 164 void AigamozuControlPackets::set_nowStatus(STATUS s)
s1210160 50:3511be172d81 165 {
s1210160 50:3511be172d81 166 nowStatus = s;
m5171135 2:3f2d4f53ceed 167 }
m5171135 5:3f51eeb5aedc 168
m5171135 5:3f51eeb5aedc 169
s1210160 50:3511be172d81 170
s1210160 50:3511be172d81 171 /////////////////////////////////////////
s1210160 50:3511be172d81 172 //
s1210160 50:3511be172d81 173 // Get
s1210160 50:3511be172d81 174 //
s1210160 50:3511be172d81 175 /////////////////////////////////////////
s1210160 50:3511be172d81 176 MODE AigamozuControlPackets::get_nowMode(void)
s1210160 50:3511be172d81 177 {
s1210160 50:3511be172d81 178 return nowMode;
s1210160 50:3511be172d81 179 }
s1210160 50:3511be172d81 180 uint8_t* AigamozuControlPackets::get_packetData(void)
s1210160 50:3511be172d81 181 {
s1210160 50:3511be172d81 182 return packetData;
s1210160 50:3511be172d81 183 }
s1210160 50:3511be172d81 184 int AigamozuControlPackets::get_packetLength(void)
s1210160 50:3511be172d81 185 {
s1210160 50:3511be172d81 186 return packetLength;
s1210160 50:3511be172d81 187 }
s1210160 50:3511be172d81 188 bool AigamozuControlPackets::get_out_flag(void)
s1210160 50:3511be172d81 189 {
s1210160 50:3511be172d81 190 return out_flag;
s1210160 50:3511be172d81 191 }
s1210160 50:3511be172d81 192 double AigamozuControlPackets::get_agzPoint_latitude(void)
s1210160 50:3511be172d81 193 {
s1210160 50:3511be172d81 194 return agzPoint.y;
s1210160 50:3511be172d81 195 }
s1210160 50:3511be172d81 196 double AigamozuControlPackets::get_agzPoint_longitude(void)
s1210160 50:3511be172d81 197 {
s1210160 50:3511be172d81 198 return agzPoint.x;
s1210160 50:3511be172d81 199 }
s1210160 50:3511be172d81 200 double AigamozuControlPackets::get_basePoint_latitude(int i)
s1210160 50:3511be172d81 201 {
s1210160 50:3511be172d81 202 return basePoint[i].y;
s1210160 50:3511be172d81 203 }
s1210160 50:3511be172d81 204 double AigamozuControlPackets::get_basePoint_longitude(int i)
s1210160 50:3511be172d81 205 {
s1210160 50:3511be172d81 206 return basePoint[i].x;
s1210160 50:3511be172d81 207 }